diff --git a/control/gcs/develop/xtdrone_qt/README.md b/control/gcs/develop/xtdrone_qt/README.md
new file mode 100644
index 0000000..e69de29
diff --git a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/formrviz.h b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/formrviz.h
new file mode 100644
index 0000000..302e47a
--- /dev/null
+++ b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/formrviz.h
@@ -0,0 +1,29 @@
+#ifndef FORMRVIZ_H
+#define FORMRVIZ_H
+
+#include <QWidget>
+
+namespace Ui {
+class FormRviz;
+}
+
+class FormRviz : public QWidget
+{
+    Q_OBJECT
+
+public:
+    explicit FormRviz(QWidget *parent = nullptr);
+    ~FormRviz();
+
+private slots:
+    void on_button_image_cancel_clicked();
+    void on_button_image_ok_clicked();
+
+signals:
+    void rviz_to_main(QString);
+
+private:
+    Ui::FormRviz *ui;
+};
+
+#endif // FORMRVIZ_H
diff --git a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/main_window.hpp b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/main_window.hpp
index c1024e8..b11f89d 100644
--- a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/main_window.hpp
+++ b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/main_window.hpp
@@ -16,6 +16,7 @@
 #include "ui_main_window.h"
 #include "qnode.hpp"
 #include "form.h"
+#include "formrviz.h"
 #include "qrviz.hpp"
 #include <QtCharts/QChart>
 #include <QtCharts/QChartView>
@@ -24,6 +25,8 @@
 #include <QProcess>
 #include <QColor>
 #include <QComboBox>
+#include <QSpinBox>
+#include <QSignalMapper>
 
 /*****************************************************************************
 ** Namespace
@@ -86,6 +89,21 @@ public:
     int plot_count = 0;
     int randnum;
     QColor color;
+    QComboBox* fixed_box;
+    QSpinBox* Cell_count_box;
+    QComboBox* Grid_color_box;
+    QComboBox* Laser_topic_box;
+    std::vector<QCheckBox*> Pose_Check;
+    std::vector<QCheckBox*> Image_Check;
+    std::vector<QComboBox*> Image_topic_box;
+    std::vector<QComboBox*> Pose_topic_box;
+    std::vector<QComboBox*> Pose_color_box;
+    std::vector<QComboBox*> Pose_shape_box;
+    QCheckBox* Tf_showname_Check;
+    int pose_num=0;
+    int image_num=0;
+    QSignalMapper* signalMapper_image;
+    QSignalMapper* signalMapper_pose;
 
 
     void ReadSettings(); // Load up qt program settings at startup
@@ -115,7 +133,17 @@ public Q_SLOTS:
     void slot_quick_output();
     void slot_box_maps_change(const QString);
     void slot_update_plot(float, float);
-
+    void slot_treewidget_value_change(QString);
+    void slot_display_grid(int);
+    void slot_display_tf(int);
+    void slot_display_Laser(int);
+    void slot_display_Image(int);
+    void slot_display_Pose(int);
+    void slot_btn_add_click(bool);
+    void slot_addrviz(QString);
+    void slot_btn_estimate_click(bool);
+    void slot_btn_goal_click(bool);
+    void slot_rviz_control();
 private:
     void init_uisettings();
     Ui::MainWindowDesign ui;
diff --git a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qnode.hpp b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qnode.hpp
index 3511517..441a8cf 100644
--- a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qnode.hpp
+++ b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qnode.hpp
@@ -32,6 +32,9 @@
 #include <std_msgs/String.h>
 //#include <boost/bind.hpp>
 #include <vector>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Path.h>
 
 
 /*****************************************************************************
@@ -79,6 +82,11 @@ public:
     void stop_control(bool q_stop_flag);
     void get_uav_control(LISTINT multirotor_get_control);
     void run();
+    geometry_msgs::Pose goal_pose;
+    bool get_goal_flag = false;
+    double roll, pitch, yaw;
+    tf::Quaternion quat;
+    int count_control = 0;
 
 
 
@@ -100,6 +108,7 @@ Q_SIGNALS:
     void loggingUpdated();
     void rosShutdown();
     void uavposition(float posi_x, float posi_y);
+    void rvizsetgoal();
 
 private:
     int init_argc;
@@ -115,7 +124,12 @@ private:
     ros::Publisher leader_cmd_vel_flu_pub;
     ros::Publisher leader_cmd_accel_flu_pub;
     ros::Publisher leader_cmd_pub;
+    ros::Publisher path_pub;
     void publish();
+    ros::Subscriber goal_sub;
+    void goal_callback(const geometry_msgs::PoseStamped &msg);
+    geometry_msgs::Twist get_uav_control(double distance, int control_num);
+    double pos2ang(double xa, double ya, double xb, double yb);
 
 };
 
diff --git a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qrviz.hpp b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qrviz.hpp
index 9802ff3..c217cf0 100644
--- a/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qrviz.hpp
+++ b/control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qrviz.hpp
@@ -14,8 +14,25 @@ class qrviz
 {
 public:
     qrviz(QVBoxLayout* layout);
+    void Set_FixedFrame(QString Frame_name);
+    void Display_Grid(int Cell_count, QColor color, bool enable);
+    void Display_TF(bool enable1, bool enable);
+    void Display_LaserScan(QString laser_topic, bool enable);
+    void Display_Image(QString Image_topic, bool enable, int num);
+    void Display_Pose(QString pose_topic, QString pose_shape, QColor color, bool enable, int num);
+    void Set_start_pose();
+    void Set_goal_pose();
 private:
     rviz::RenderPanel* render_panel;
+    rviz::VisualizationManager* manager;
+    rviz::Display* Grid = NULL;
+    rviz::Display* Tf = NULL;
+    rviz::Display* Laser = NULL;
+    std::vector<rviz::Display*> Image;
+    std::vector<rviz::Display*> Pose;
+    rviz::ToolManager* tool_manager;
+    int count_pose_num[20]={0};
+    int count_image_num[20]={0};
 };
 
 #endif // QRVIZ_HPP
diff --git a/control/gcs/develop/xtdrone_qt/src/formrviz.cpp b/control/gcs/develop/xtdrone_qt/src/formrviz.cpp
new file mode 100644
index 0000000..0fb9cb3
--- /dev/null
+++ b/control/gcs/develop/xtdrone_qt/src/formrviz.cpp
@@ -0,0 +1,49 @@
+#include "../include/xtdrone_qt/formrviz.h"
+#include "ui_formrviz.h"
+
+FormRviz::FormRviz(QWidget *parent) :
+    QWidget(parent),
+    ui(new Ui::FormRviz)
+{
+    ui->setupUi(this);
+}
+
+FormRviz::~FormRviz()
+{
+    delete ui;
+}
+
+
+void FormRviz::on_button_image_cancel_clicked()
+{
+    this->hide();
+}
+
+void FormRviz::on_button_image_ok_clicked()
+{
+    if (ui->button_image_camera->isChecked())
+    {
+        emit rviz_to_main("image");
+        this->hide();
+    }
+    else if(ui->button_image_tf->isChecked())
+    {
+        emit rviz_to_main("tf");
+        this->hide();
+    }
+    else if(ui->button_image_laser->isChecked())
+    {
+        emit rviz_to_main("laser");
+        this->hide();
+    }
+    else if(ui->button_image_addtopic->isChecked())
+    {
+        QString topic = ui->edit_addtopic->text();
+        emit rviz_to_main(topic);
+        this->hide();
+    }
+    else
+        this->hide();
+
+
+}
diff --git a/control/gcs/develop/xtdrone_qt/src/main_window.cpp b/control/gcs/develop/xtdrone_qt/src/main_window.cpp
index c3204ac..074c821 100644
--- a/control/gcs/develop/xtdrone_qt/src/main_window.cpp
+++ b/control/gcs/develop/xtdrone_qt/src/main_window.cpp
@@ -107,15 +107,41 @@ MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
     QTreeWidgetItem* Global = new QTreeWidgetItem(QStringList()<<"Global Options");
     ui.treeWidget->addTopLevelItem(Global);
     Global->setExpanded(true);
+    // FixFrame
     QTreeWidgetItem* Fixed_frame = new QTreeWidgetItem(QStringList()<<"Fixed Frame");
-    QComboBox* fixed_box = new QComboBox();
+    fixed_box = new QComboBox();
     fixed_box->addItem("map");
-    fixed_box->setMaximumWidth(100);
+    fixed_box->addItem("base_footprint");
+    fixed_box->addItem("laser");
+    fixed_box->setMaximumWidth(120);
     fixed_box->setEditable(true);
+    connect(fixed_box,SIGNAL(currentTextChanged(QString)),this,SLOT(slot_treewidget_value_change(QString)));
     Global->addChild(Fixed_frame);
     ui.treeWidget->setItemWidget(Fixed_frame,1,fixed_box);
     ui.treeWidget->header()->setMaximumSectionSize(150);
     ui.treeWidget->header()->setDefaultSectionSize(150);
+    // Grid
+    QTreeWidgetItem* Grid = new QTreeWidgetItem(QStringList()<<"Grid");
+    QCheckBox* Grid_Check = new QCheckBox();
+    connect(Grid_Check, SIGNAL(stateChanged(int)), this, SLOT(slot_display_grid(int)));
+    ui.treeWidget->addTopLevelItem(Grid);
+    ui.treeWidget->setItemWidget(Grid,1,Grid_Check);
+    Grid->setExpanded(true);
+    QTreeWidgetItem* Cell_count = new QTreeWidgetItem(QStringList()<<"Cell Count");
+    Grid->addChild(Cell_count);
+    Cell_count_box = new QSpinBox();
+    Cell_count_box->setValue(15);
+    Cell_count_box->setMaximumWidth(120);
+    ui.treeWidget->setItemWidget(Cell_count,1,Cell_count_box);
+    QTreeWidgetItem* Grid_color = new QTreeWidgetItem(QStringList()<<"Color");
+    Grid->addChild(Grid_color);
+    Grid_color_box = new QComboBox();
+    Grid_color_box->addItem("160;160;160");
+    Grid_color_box->setEditable(true);
+    Grid_color_box->setMaximumWidth(120);
+    ui.treeWidget->setItemWidget(Grid_color,1,Grid_color_box);
+    signalMapper_image = new QSignalMapper(this);
+    signalMapper_pose = new QSignalMapper(this);
 
 }
 
@@ -184,6 +210,10 @@ void MainWindow::ReadSettings() {
     connect(ui.button_connect,SIGNAL(clicked(bool)),this,SLOT(slot_btn_connect_click(bool)));
     connect(ui.comboBox_maps,SIGNAL(currentTextChanged(const QString)),this,SLOT(slot_box_maps_change(const QString)));
     connect(&qnode, SIGNAL(uavposition(float, float)),this,SLOT(slot_update_plot(float, float)));
+    connect(ui.button_add, SIGNAL(clicked(bool)),this,SLOT(slot_btn_add_click(bool)));
+    connect(ui.button_estimate, SIGNAL(clicked(bool)),this,SLOT(slot_btn_estimate_click(bool)));
+    connect(ui.button_goal, SIGNAL(clicked(bool)),this,SLOT(slot_btn_goal_click(bool)));
+    connect(&qnode, SIGNAL(rvizsetgoal()),this,SLOT(slot_rviz_control()));
 }
 
 void MainWindow::WriteSettings() {
@@ -208,7 +238,6 @@ void MainWindow::Seriesint(int ch)
     color.setBlue(randnum);
     color_plot.push_back(color);
     seriesList[ch]->setPen(QPen(color,2,Qt::SolidLine));
-    qDebug()<<"colot_polt";
     chart->setAxisX(axisX, seriesList[ch]);
     chart->setAxisY(axisY, seriesList[ch]);
 }
@@ -238,6 +267,9 @@ void MainWindow::init_uisettings()
     ui.checkBox_quadplane->setCheckable(false);
     ui.checkBox_tiltrotor->setCheckable(false);
     ui.checkBox_tailsitter->setCheckable(false);
+//    ui.button_add->setChecked(false);
+//    ui.button_add->setEnabled(false);
+
     once_flag = 0;
     twice_flag = 0;
     ui.box_go_and_back_2->setProperty("value", 0.0);
@@ -397,10 +429,13 @@ void MainWindow::slot_btn_run_click(bool)
 
             if ( !qnode.init(multirotor_select, multirotor_num, multi_type,control_type.toStdString())) {   //toStdString()
                 showNoMasterMessage();
+                ui.treeWidget->setEnabled(false);
             } else {
                 ui.button_run->setEnabled(false);
                 ui.button_control->setEnabled(true);
                 my_rviz = new qrviz(ui.verticalLayout_rviz);
+                ui.treeWidget->setEnabled(true);
+//                ui.button_add->setEnabled(true);
             }
         }
 
@@ -435,7 +470,7 @@ void MainWindow::slot_quick_output()
     ui.launch_info_text->append("<font color=\"#FF0000\">"+launch_cmd->readAllStandardError()+"</font>");
     ui.launch_info_text->append("<font color=\"#000000\">"+launch_cmd->readAllStandardOutput()+"</font>");
 }
-void MainWindow::slot_box_valuechange(double value)
+void MainWindow::slot_box_valuechange(double)
 {
     qDebug()<<"change!!!";
     double forward_change = ui.box_go_and_back_2->value();
@@ -448,7 +483,7 @@ void MainWindow::slot_box_valuechange(double value)
     qnode.set_cmd(forward_change, upward_change, leftward_change, orientation_change, ctrl_leader, cmd3, cmd_vel_mask);
 }
 
-void MainWindow::box_command_valuechange(const QString value)
+void MainWindow::box_command_valuechange(const QString)
 {
     qDebug()<<"change!!!";
     double forward_change = ui.box_go_and_back_2->value();
@@ -580,7 +615,7 @@ QString MainWindow::box_formation_valuechange(const QString formation_mid)
     return formation2;
 }
 
-void MainWindow::slot_checkbox_get_uav_control(int value)
+void MainWindow::slot_checkbox_get_uav_control(int)
 {
     multirotor_get_control.clear();
     for (int i = 0; i<multirotor_num[0] ;i++)
@@ -737,9 +772,164 @@ void MainWindow::slot_update_plot(float x_position, float y_position)
     seriesList[plot_count]->append(x_position, y_position);
 //    if (plot_count==1 ||plot_count ==5)
 //    {
-        qDebug()<<"series"<<plot_count<<":"<<x_position;
-        qDebug()<<"series"<<plot_count<<":"<<y_position;
+//        qDebug()<<"series"<<plot_count<<":"<<x_position;
+//        qDebug()<<"series"<<plot_count<<":"<<y_position;
 //    }
     plot_count ++;
 }
+
+void MainWindow::slot_treewidget_value_change(QString)
+{
+    my_rviz->Set_FixedFrame(fixed_box->currentText());
+}
+
+void MainWindow::slot_display_grid(int state)
+{
+    bool enable=state>1?true:false;
+    QStringList cut_color = Grid_color_box->currentText().split(";");
+    QColor color = QColor(cut_color[0].toInt(),cut_color[1].toInt(),cut_color[2].toInt());
+    my_rviz->Display_Grid(Cell_count_box->text().toInt(), color, enable);
+}
+
+void MainWindow::slot_display_tf(int state)
+{
+    bool enable=state>1?true:false;
+    int state1 = Tf_showname_Check->checkState();
+    bool enable1=state1>1?true:false;
+    my_rviz->Display_TF(enable1, enable);
+}
+
+void MainWindow::slot_display_Laser(int state)
+{
+    bool enable=state>1?true:false;
+    my_rviz->Display_LaserScan(Laser_topic_box->currentText(),enable);
+}
+
+void MainWindow::slot_display_Image(int num)
+{
+    bool enable = Image_Check[num]->checkState()>1?true:false;
+    my_rviz->Display_Image(Image_topic_box[num]->currentText(),enable, num);
+}
+
+void MainWindow::slot_display_Pose(int num)
+{
+    bool enable = Pose_Check[num]->checkState()>1?true:false;
+    QStringList cut_color = Pose_color_box[num]->currentText().split(";");
+    QColor color = QColor(cut_color[0].toInt(),cut_color[1].toInt(),cut_color[2].toInt());
+    my_rviz->Display_Pose(Pose_topic_box[num]->currentText(),Pose_shape_box[num]->currentText(),color,enable,num);
+    qDebug()<<"test!!!";
+}
+
+void MainWindow::slot_btn_add_click(bool)
+{
+    FormRviz* frviz=new FormRviz;
+    frviz->show();
+    connect(frviz,SIGNAL(rviz_to_main(QString)),this,SLOT(slot_addrviz(QString)));
+}
+
+void MainWindow::slot_addrviz(QString value)
+{
+    qDebug()<<value;
+    if (value == "tf")
+    {
+        //TF
+        QTreeWidgetItem* TF = new QTreeWidgetItem(QStringList()<<"TF");
+        QCheckBox* TF_Check = new QCheckBox();
+        connect(TF_Check, SIGNAL(stateChanged(int)), this, SLOT(slot_display_tf(int)));
+        ui.treeWidget->addTopLevelItem(TF);
+        ui.treeWidget->setItemWidget(TF,1,TF_Check);
+        QTreeWidgetItem* Tf_showname = new QTreeWidgetItem(QStringList()<<"Show Names");
+        TF->addChild(Tf_showname);
+        Tf_showname_Check = new QCheckBox();
+        ui.treeWidget->setItemWidget(Tf_showname,1,Tf_showname_Check);
+        Tf_showname_Check->setCheckState(Qt::CheckState(2));
+    }
+    else if (value == "laser")
+    {
+        //Laser
+        QTreeWidgetItem* Laser = new QTreeWidgetItem(QStringList()<<"Laser Scan");
+        QCheckBox* Laser_Check = new QCheckBox();
+        connect(Laser_Check, SIGNAL(stateChanged(int)), this, SLOT(slot_display_Laser(int)));
+        ui.treeWidget->addTopLevelItem(Laser);
+        ui.treeWidget->setItemWidget(Laser,1,Laser_Check);
+        QTreeWidgetItem* Laser_topic = new QTreeWidgetItem(QStringList()<<"Topic");
+        Laser->addChild(Laser_topic);
+        Laser_topic_box = new QComboBox();
+        Laser_topic_box->addItem("/scan");
+        Laser_topic_box->setEditable(true);
+        Laser_topic_box->setMaximumWidth(120);
+        ui.treeWidget->setItemWidget(Laser_topic,1,Laser_topic_box);
+    }
+    else if (value == "image")
+    {
+        //Image
+        QTreeWidgetItem* Image = new QTreeWidgetItem(QStringList()<<"Image");
+        Image_Check.push_back(new QCheckBox());
+        connect(Image_Check[image_num], SIGNAL(stateChanged(int)), signalMapper_image, SLOT(map()));
+        signalMapper_image->setMapping(Image_Check[image_num],image_num);
+        connect(signalMapper_image,SIGNAL(mapped(int)),this,SLOT(slot_display_Image(int)));
+        ui.treeWidget->addTopLevelItem(Image);
+        ui.treeWidget->setItemWidget(Image,1,Image_Check[image_num]);
+        QTreeWidgetItem* Image_topic = new QTreeWidgetItem(QStringList()<<"Topic");
+        Image->addChild(Image_topic);
+        Image_topic_box.push_back( new QComboBox());
+        Image_topic_box[image_num]->addItem("");
+        Image_topic_box[image_num]->setEditable(true);
+        Image_topic_box[image_num]->setMaximumWidth(120);
+        ui.treeWidget->setItemWidget(Image_topic,1,Image_topic_box[image_num]);
+        image_num++;
+    }
+    else
+    {
+        value.remove(QChar('\n'), Qt::CaseInsensitive);
+        //Add topic
+        QTreeWidgetItem* Pose = new QTreeWidgetItem(QStringList()<<"Pose");
+        Pose_Check.push_back(new QCheckBox());
+        connect(Pose_Check[pose_num], SIGNAL(stateChanged(int)), signalMapper_pose, SLOT(map()));
+        signalMapper_pose->setMapping(Pose_Check[pose_num],pose_num);
+        connect(signalMapper_pose,SIGNAL(mapped(int)),this,SLOT(slot_display_Pose(int)));
+        ui.treeWidget->addTopLevelItem(Pose);
+        ui.treeWidget->setItemWidget(Pose,1,Pose_Check[pose_num]);
+        QTreeWidgetItem* Pose_topic = new QTreeWidgetItem(QStringList()<<"Topic");
+        Pose->addChild(Pose_topic);
+        Pose_topic_box.push_back(new QComboBox());
+        Pose_topic_box[pose_num]->addItem(value);
+        Pose_topic_box[pose_num]->setEditable(true);
+        Pose_topic_box[pose_num]->setMaximumWidth(200);
+        ui.treeWidget->setItemWidget(Pose_topic,1,Pose_topic_box[pose_num]);
+        QTreeWidgetItem* Pose_color = new QTreeWidgetItem(QStringList()<<"Color");
+        Pose->addChild(Pose_color);
+        Pose_color_box.push_back(new QComboBox());
+        Pose_color_box[pose_num]->addItem("160;160;160");
+        Pose_color_box[pose_num]->setEditable(true);
+        Pose_color_box[pose_num]->setMaximumWidth(120);
+        ui.treeWidget->setItemWidget(Pose_color,1,Pose_color_box[pose_num]);
+        QTreeWidgetItem* Pose_shape = new QTreeWidgetItem(QStringList()<<"Shape");
+        Pose->addChild(Pose_shape);
+        Pose_shape_box.push_back(new QComboBox());
+        Pose_shape_box[pose_num]->addItem("Axes");
+        Pose_shape_box[pose_num]->addItem("Arrow");
+        Pose_shape_box[pose_num]->setEditable(true);
+        Pose_shape_box[pose_num]->setMaximumWidth(120);
+        ui.treeWidget->setItemWidget(Pose_shape,1,Pose_shape_box[pose_num]);
+        pose_num++;
+    }
+}
+
+void MainWindow::slot_btn_estimate_click(bool)
+{
+    my_rviz->Set_start_pose();
+}
+
+void MainWindow::slot_btn_goal_click(bool)
+{
+    my_rviz->Set_goal_pose();
+}
+void MainWindow::slot_rviz_control()
+{
+    ui.box_go_and_back_2->setProperty("value", 0.0);
+    ui.box_up_and_down_2->setProperty("value", 0.0);
+    ui.box_left_and_right_2->setProperty("value", 0.0);
+    ui.box_orientation->setProperty("value", 0.0);
+}
 }  // namespace xtdrone_qt
diff --git a/control/gcs/develop/xtdrone_qt/src/qnode.cpp b/control/gcs/develop/xtdrone_qt/src/qnode.cpp
index c5cb7bd..34bcbb2 100644
--- a/control/gcs/develop/xtdrone_qt/src/qnode.cpp
+++ b/control/gcs/develop/xtdrone_qt/src/qnode.cpp
@@ -18,6 +18,8 @@
 #include "../include/xtdrone_qt/qnode.hpp"
 //#include <boost/bind.hpp>
 #include "QDebug"
+#include <math.h>
+
 
 typedef QList<geometry_msgs::Twist> LISTTWIST;
 typedef QList<geometry_msgs::Pose> LISTPOSE;
@@ -51,9 +53,9 @@ QNode::~QNode() {
 *****************************************************************************/
 
 
-void odm_groundtruth_callback(const nav_msgs::Odometry::ConstPtr& msg, int num_odm, geometry_msgs::Pose local_pose[])
+void odm_groundtruth_callback(const geometry_msgs::PoseStamped::ConstPtr& msg, int num_odm, geometry_msgs::Pose local_pose[])
 {
-    local_pose[num_odm] = msg->pose.pose;
+    local_pose[num_odm] = msg->pose;
 //    qDebug()<<"pose::"<<num_odm<<"::"<<local_pose[num_odm].position.y;
 }
 
@@ -115,6 +117,8 @@ bool QNode::init(const LISTINT multi_select, const int *multi_num, const LISTSTR
 
     // Add your ros communications here.
     chatter_publisher = n.advertise<std_msgs::String>("chatter_0", 1000);
+    goal_sub = n.subscribe("/move_base_simple/goal", 1000, &QNode::goal_callback, this);
+    path_pub = n.advertise<nav_msgs::Path>("trajectory", 1, true);
     if (control_type == "vel")
     {
         for (int i = 0; i < leng_select; i ++)
@@ -129,7 +133,7 @@ bool QNode::init(const LISTINT multi_select, const int *multi_num, const LISTSTR
                 }
                 else
                 {
-                    buffer_multi_cmd_vel_flu_pub.push_back(n.advertise<geometry_msgs::Twist>("/xtdrone/"+multi_type[counnnnnt]+"_"+std::to_string(k)+"/cmd_vel_flu", 1000));
+                    buffer_multi_cmd_vel_flu_pub.push_back(n.advertise<geometry_msgs::Twist>("/xtdrone/"+multi_type[counnnnnt]+"_"+std::to_string(k)+"/cmd_vel_enu", 1000));
                     buffer_multi_cmd_pub.push_back(n.advertise<std_msgs::String>("/xtdrone/"+multi_type[counnnnnt]+"_"+std::to_string(k)+"/cmd",1000));
                     qDebug()<<"multi_type"<<multi_type[counnnnnt].c_str();
                     counnnnnt++;
@@ -161,7 +165,7 @@ bool QNode::init(const LISTINT multi_select, const int *multi_num, const LISTSTR
     {
         for (int k = 0; k < multi_num[multi_select[i]]; k ++)
         {
-            buffer_multi_odom_groundtruth_sub.push_back(n.subscribe<nav_msgs::Odometry>("/xtdrone/"+multi_type[counnnt]+'_'+std::to_string(k)+"/ground_truth/odom", 1000, boost::bind(&odm_groundtruth_callback, _1, counnnt, local_pose)));
+            buffer_multi_odom_groundtruth_sub.push_back(n.subscribe<geometry_msgs::PoseStamped>(multi_type[counnnt]+'_'+std::to_string(k)+"/mavros/local_position/pose", 1000, boost::bind(&odm_groundtruth_callback, _1, counnnt, local_pose)));
             counnnt ++;
         }
     }
@@ -218,6 +222,17 @@ void QNode::start_control(bool q_start_control_flag)
     qDebug()<<"control!!!";
 }
 
+void QNode::goal_callback(const geometry_msgs::PoseStamped &msg)
+{
+    goal_pose.position.x  = msg.pose.position.x;
+    goal_pose.position.y = msg.pose.position.y;
+    get_goal_flag = true;
+    qDebug()<<"goal x:"<<goal_pose.position.x;
+    qDebug()<<"goal y:"<<goal_pose.position.y;
+    qDebug()<<"current x:"<<local_pose[0].position.x;
+    qDebug()<<"current y:"<<local_pose[0].position.y;
+    emit rvizsetgoal();
+}
 // functions of ROS
 void QNode::run() {
     ros::Rate loop_rate(100);
@@ -225,11 +240,39 @@ void QNode::run() {
     std::string text_all;
     std::string text[multirotor_num];
     std_msgs::String msg;
+    geometry_msgs::Twist vel_goal;
+    bool arrive_flag = false;
+    int arrive_count = 0;
+    int control_num = 0;
+    ros::Time current_time, last_time;
+    current_time = ros::Time::now();
+    last_time = ros::Time::now();
+    nav_msgs::Path path;
+    //nav_msgs::Path path;
+    path.header.stamp = current_time;
+    path.header.frame_id = "odom";
+
 //    geometry_msgs::Twist twist[multirotor_num];
     qDebug()<<"start!!!";
     while ( ros::ok() ) {
-        // show log:
+        //rviz draw path
+        current_time = ros::Time::now();
+        geometry_msgs::PoseStamped this_pose_stamped;
+        this_pose_stamped.pose.position.x = local_pose[0].position.x;
+        this_pose_stamped.pose.position.y = local_pose[0].position.y;
+        this_pose_stamped.pose.position.z = local_pose[0].position.z;
+        this_pose_stamped.pose.orientation.x = local_pose[0].orientation.x;
+        this_pose_stamped.pose.orientation.y = local_pose[0].orientation.y;
+        this_pose_stamped.pose.orientation.z = local_pose[0].orientation.z;
+        this_pose_stamped.pose.orientation.w = local_pose[0].orientation.w;
+        this_pose_stamped.header.stamp = current_time;
+        this_pose_stamped.header.frame_id = "odom";
+        path.poses.push_back(this_pose_stamped);
+        path_pub.publish(path);
+        last_time = current_time;
+
         text_all = "";
+        // show log:
         if (count%30 == 0)
         {
             if (multirotor_num < 10)
@@ -248,6 +291,76 @@ void QNode::run() {
                 log(Info,msg.data);
             }
         }
+        if (get_goal_flag)
+        {
+            for (int i = 0; i < multirotor_num; i ++)
+            {
+                if (multirotor_get_control[i])
+                {
+                    control_num = i;
+                    break;
+                }
+            }
+            double distance_tar_cur = (pow((goal_pose.position.x - local_pose[control_num].position.x),2)+pow((goal_pose.position.y - local_pose[control_num].position.y),2));
+            if (count_control%30 == 0)
+            {
+                qDebug()<< "distance" << distance_tar_cur;
+                qDebug()<<"arrive flag"<<arrive_flag;
+                qDebug()<<"current pose x"<<local_pose[control_num].position.x;
+                qDebug()<<"current pose y"<<local_pose[control_num].position.y;
+            }
+            if  (distance_tar_cur < 0.1)
+            {
+                arrive_count += 1;
+                if (arrive_count > 3)
+                {
+                    arrive_flag = true;
+                    arrive_count = 0;
+                    get_goal_flag = false;
+                }
+                else
+                    arrive_flag = false;
+            }
+            else
+            {
+                arrive_count = 0;
+                arrive_flag = false;
+            }
+            if (!arrive_flag)
+            {
+                tf::quaternionMsgToTF(local_pose[control_num].orientation, quat);
+                tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
+//                qDebug()<<"yaw:"<<yaw;
+                vel_goal = get_uav_control(distance_tar_cur,control_num);
+                for (int i = 0; i < multirotor_num; i ++)
+                {
+                    cmd[i].data = "";
+                    if (multirotor_get_control[i])
+                    {
+                        vel[i].linear.x = vel_goal.linear.x;
+                        vel[i].linear.y = vel_goal.linear.y;
+                        vel[i].linear.z = vel_goal.linear.z;
+                        vel[i].angular.z = vel_goal.angular.z;
+                    }
+                }
+            }
+            else
+            {
+                for (int i = 0; i < multirotor_num; i ++)
+                {
+                    cmd[i].data = "";
+                    if (multirotor_get_control[i])
+                    {
+                        vel[i].linear.x = 0.0;
+                        vel[i].linear.y = 0.0;
+                        vel[i].linear.z = 0.0;
+                        vel[i].angular.z = 0.0;
+                    }
+                }
+            }
+            QNode::publish();
+        }
+
         if (start_control_flag)
         {
             // update cmd:
@@ -270,7 +383,10 @@ void QNode::run() {
                 }
             }
             cmd_change_flag = false;
-            QNode::publish();
+            if (!get_goal_flag)
+            {
+                QNode::publish();
+            }
         }
         if (stop_flag)
         {
@@ -357,4 +473,69 @@ void QNode::log( const LogLevel &level, const std::string &msg) {
     Q_EMIT loggingUpdated(); // used to readjust the scrollbar
 }
 
+geometry_msgs::Twist QNode::get_uav_control(double distance, int control_num)
+{
+    geometry_msgs::Twist vel_goal;
+    double uav_vel_total =  0.5*distance;
+    if (uav_vel_total > 2.0)
+        uav_vel_total = 2.0;
+    double target_yaw = pos2ang(goal_pose.position.x, goal_pose.position.y, local_pose[control_num].position.x, local_pose[control_num].position.y);
+    count_control ++;
+    double mid_yaw = target_yaw - yaw;
+    if (mid_yaw > 3.1415926)
+         mid_yaw = mid_yaw - 3.1415926*2;
+    else if(mid_yaw < -3.1415926)
+        mid_yaw = 2*3.1415926 + mid_yaw;
+    vel_goal.angular.x = 0.0;
+    vel_goal.angular.y = 0.0;
+    vel_goal.angular.z = 1.0* mid_yaw;
+    if (vel_goal.angular.z > 3.0)
+        vel_goal.angular.z = 3.0;
+    else if (vel_goal.angular.z < -3.0)
+        vel_goal.angular.z = -3.0;
+    vel_goal.linear.x  = uav_vel_total * cos(target_yaw);
+    vel_goal.linear.y = uav_vel_total * sin(target_yaw);
+    vel_goal.linear.z = 0.0;
+    if (count_control %30 == 0)
+    {
+    qDebug()<<"target_yaw"<<target_yaw;
+    qDebug()<<"current_yaw"<<yaw;
+    qDebug()<<"vel_goal x" << vel_goal.linear.x;
+    qDebug()<<"vel_goal y" << vel_goal.linear.y;
+    qDebug()<<"vel_goal ang" << vel_goal.angular.z;
+    }
+    return vel_goal;
+}
+
+double QNode::pos2ang(double xa, double ya, double xb, double yb)
+{
+    double angle = 0.0;
+        if (xa-xb != 0)
+        {
+           angle = atan2((ya - yb),(xa - xb));
+            if (ya-yb > 0 && angle < 0)
+                angle = angle + 3.1415926;
+            else if (ya-yb < 0 && angle > 0)
+                angle = angle - 3.1415926;
+            else if (ya-yb == 0)
+            {
+                if (xa-xb > 0)
+                    angle = 0.0;
+                else
+                    angle = 3.1415926;
+            }
+        }
+        else
+        {
+            if (ya-yb > 0)
+                angle = 3.1415926 / 2;
+            else if (ya-yb <0)
+                angle = -3.1415926 / 2;
+            else
+                angle = 0.0;
+        }
+        if (angle < 0)
+            angle = angle + 2 * 3.1415926;
+        return angle;
+}
 }  // namespace xtdrone_qt
diff --git a/control/gcs/develop/xtdrone_qt/src/qrviz.cpp b/control/gcs/develop/xtdrone_qt/src/qrviz.cpp
index 61b6ab5..f76b4d3 100644
--- a/control/gcs/develop/xtdrone_qt/src/qrviz.cpp
+++ b/control/gcs/develop/xtdrone_qt/src/qrviz.cpp
@@ -1,8 +1,115 @@
 #include "../include/xtdrone_qt/qrviz.hpp"
+#include <QDebug>
 
 qrviz::qrviz(QVBoxLayout* layout)
 {
     // create rviz
     render_panel = new rviz::RenderPanel();
     layout->addWidget(render_panel);
+    manager = new rviz::VisualizationManager(render_panel);
+    tool_manager = manager->getToolManager();
+    ROS_ASSERT(manager!=NULL);
+    render_panel->initialize(manager->getSceneManager(),manager);  // must before mananer->init
+    //init rviz
+    manager->initialize();
+    manager->startUpdate();
+    manager->removeAllDisplays();
+
+}
+void qrviz::Set_FixedFrame(QString Fixed_name)
+{
+    manager->setFixedFrame(Fixed_name);
+    qDebug()<<manager->getFixedFrame();
+}
+void qrviz::Display_Grid(int Cell_count, QColor color, bool enable)
+{
+    if (Grid!=NULL)  // only one grid is added on rviz
+    {
+        delete Grid;
+        Grid = NULL;
+    }
+   Grid = manager->createDisplay("rviz/Grid","myGrid",enable);
+   Grid->subProp("Plane Cell Count")->setValue(Cell_count);
+   Grid->subProp("Color")->setValue(color);
+   ROS_ASSERT(Grid!=NULL);
+   manager->startUpdate();
+}
+void qrviz::Display_Pose(QString pose_topic, QString pose_shape, QColor color, bool enable, int num)
+{
+    if (count_pose_num[num]==0)
+    {
+        count_pose_num[num]++;
+        Pose.push_back(NULL);
+    }
+    else
+    {
+        if (Pose[num]!=NULL)  // only one grid is added on rviz
+        {
+            delete Pose[num];
+            Pose[num] = NULL;
+        }
+    }
+   Pose[num] = manager->createDisplay("rviz/Pose","myPose",enable);
+   Pose[num]->subProp("Topic")->setValue(pose_topic);
+   Pose[num]->subProp("Color")->setValue(color);
+   Pose[num]->subProp("Shape")->setValue(pose_shape);
+   ROS_ASSERT(Pose[num]!=NULL);
+   manager->startUpdate();
+}
+void qrviz::Display_TF(bool enable1, bool enable)
+{
+    if (Tf!=NULL)  // only one grid is added on rviz
+    {
+        delete Tf;
+        Tf = NULL;
+    }
+   Tf = manager->createDisplay("rviz/TF","myTF",enable);
+   Tf->subProp("Show Names")->setValue(enable1);
+   ROS_ASSERT(Tf!=NULL);
+   manager->startUpdate();
+}
+void qrviz::Display_LaserScan(QString laser_topic, bool enable)
+{
+    if (Laser!=NULL)  // only one grid is added on rviz
+    {
+        delete Laser;
+        Laser = NULL;
+    }
+   Laser = manager->createDisplay("rviz/LaserScan","myLaser",enable);
+   Laser->subProp("Topic")->setValue(laser_topic);
+   ROS_ASSERT(Laser!=NULL);
+   manager->startUpdate();
+}
+void qrviz::Display_Image(QString Image_topic, bool enable, int num)
+{
+    qDebug()<<"image_num"<<num;
+    if (count_image_num[num]==0)
+    {
+        count_image_num[num]++;
+        Image.push_back(NULL);
+    }
+    else
+    {
+        if (Image[num]!=NULL)  // only one grid is added on rviz
+        {
+            delete Image[num];
+            Image[num] = NULL;
+        }
+    }
+   Image[num] = manager->createDisplay("rviz/Image","myImage",enable);
+   Image[num]->subProp("Image Topic")->setValue(Image_topic);
+   ROS_ASSERT(Image[num]!=NULL);
+   manager->startUpdate();
+}
+void qrviz::Set_start_pose()
+{
+    rviz::Tool* current_tool = tool_manager->addTool("rviz/SetInitialPose");
+    tool_manager->setCurrentTool(current_tool);
+}
+void qrviz::Set_goal_pose()
+{
+    rviz::Tool* current_tool = tool_manager->addTool("rviz/SetGoal");
+    rviz::Property* pro = current_tool->getPropertyContainer();
+    pro->subProp("Topic")->setValue("/move_base_simple/goal");
+    tool_manager->setCurrentTool(current_tool);
 }
diff --git a/control/gcs/develop/xtdrone_qt/ui/form.ui b/control/gcs/develop/xtdrone_qt/ui/form.ui
index ccdb11c..dec0068 100644
--- a/control/gcs/develop/xtdrone_qt/ui/form.ui
+++ b/control/gcs/develop/xtdrone_qt/ui/form.ui
@@ -44,7 +44,9 @@ p, li { white-space: pre-wrap; }
     </rect>
    </property>
    <property name="styleSheet">
-    <string notr="true">font: 75 15pt &quot;Ubuntu Mono&quot;;</string>
+    <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);</string>
    </property>
    <property name="text">
     <string>Reset</string>
@@ -60,7 +62,9 @@ p, li { white-space: pre-wrap; }
     </rect>
    </property>
    <property name="styleSheet">
-    <string notr="true">font: 75 15pt &quot;Ubuntu Mono&quot;;</string>
+    <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);</string>
    </property>
    <property name="text">
     <string>Ignore</string>
diff --git a/control/gcs/develop/xtdrone_qt/ui/formrviz.ui b/control/gcs/develop/xtdrone_qt/ui/formrviz.ui
new file mode 100644
index 0000000..0b403d6
--- /dev/null
+++ b/control/gcs/develop/xtdrone_qt/ui/formrviz.ui
@@ -0,0 +1,160 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>FormRviz</class>
+ <widget class="QWidget" name="FormRviz">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>330</width>
+    <height>353</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Rviz</string>
+  </property>
+  <widget class="QFrame" name="frame">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>60</y>
+     <width>271</width>
+     <height>211</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">background-color: rgb(238, 238, 236);</string>
+   </property>
+   <property name="frameShape">
+    <enum>QFrame::StyledPanel</enum>
+   </property>
+   <property name="frameShadow">
+    <enum>QFrame::Raised</enum>
+   </property>
+   <widget class="QLineEdit" name="edit_addtopic">
+    <property name="geometry">
+     <rect>
+      <x>10</x>
+      <y>160</y>
+      <width>221</width>
+      <height>25</height>
+     </rect>
+    </property>
+    <property name="whatsThis">
+     <string>add topic here</string>
+    </property>
+   </widget>
+   <widget class="QWidget" name="layoutWidget">
+    <property name="geometry">
+     <rect>
+      <x>10</x>
+      <y>11</y>
+      <width>110</width>
+      <height>136</height>
+     </rect>
+    </property>
+    <layout class="QVBoxLayout" name="verticalLayout">
+     <item>
+      <widget class="QRadioButton" name="button_image_camera">
+       <property name="styleSheet">
+        <string notr="true">font: 57 15pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);</string>
+       </property>
+       <property name="text">
+        <string>image</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QRadioButton" name="button_image_tf">
+       <property name="styleSheet">
+        <string notr="true">font: 57 15pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);</string>
+       </property>
+       <property name="text">
+        <string>tf</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QRadioButton" name="button_image_laser">
+       <property name="styleSheet">
+        <string notr="true">font: 57 15pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);</string>
+       </property>
+       <property name="text">
+        <string>laser</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QRadioButton" name="button_image_addtopic">
+       <property name="styleSheet">
+        <string notr="true">font: 57 15pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);</string>
+       </property>
+       <property name="text">
+        <string>add topic</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </widget>
+  </widget>
+  <widget class="QPushButton" name="button_image_ok">
+   <property name="geometry">
+    <rect>
+     <x>198</x>
+     <y>300</y>
+     <width>91</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);</string>
+   </property>
+   <property name="text">
+    <string>OK</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>60</x>
+     <y>10</y>
+     <width>191</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">color: rgb(32, 74, 135);
+font: 57 23pt &quot;Ubuntu&quot;;</string>
+   </property>
+   <property name="text">
+    <string>Display types</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="button_image_cancel">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>300</y>
+     <width>91</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);</string>
+   </property>
+   <property name="text">
+    <string>Cancel</string>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/control/gcs/develop/xtdrone_qt/ui/main_window.ui b/control/gcs/develop/xtdrone_qt/ui/main_window.ui
index 08f5041..1bf58db 100644
--- a/control/gcs/develop/xtdrone_qt/ui/main_window.ui
+++ b/control/gcs/develop/xtdrone_qt/ui/main_window.ui
@@ -800,7 +800,7 @@ border-bottom-color: rgb(186, 189, 182);
        <x>0</x>
        <y>0</y>
        <width>281</width>
-       <height>651</height>
+       <height>661</height>
       </rect>
      </property>
      <property name="contextMenuPolicy">
@@ -810,7 +810,7 @@ border-bottom-color: rgb(186, 189, 182);
       <string notr="true">background-color: rgb(238, 238, 236);</string>
      </property>
      <property name="currentIndex">
-      <number>0</number>
+      <number>1</number>
      </property>
      <widget class="QWidget" name="launch_show_2">
       <attribute name="title">
@@ -837,7 +837,7 @@ border-bottom-color: rgb(186, 189, 182);
          <x>0</x>
          <y>0</y>
          <width>281</width>
-         <height>621</height>
+         <height>591</height>
         </rect>
        </property>
        <attribute name="headerMinimumSectionSize">
@@ -857,6 +857,28 @@ border-bottom-color: rgb(186, 189, 182);
         </property>
        </column>
       </widget>
+      <widget class="QPushButton" name="button_add">
+       <property name="geometry">
+        <rect>
+         <x>90</x>
+         <y>600</y>
+         <width>89</width>
+         <height>25</height>
+        </rect>
+       </property>
+       <property name="whatsThis">
+        <string>Add more display types</string>
+       </property>
+       <property name="styleSheet">
+        <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);
+</string>
+       </property>
+       <property name="text">
+        <string>Add</string>
+       </property>
+      </widget>
      </widget>
     </widget>
    </widget>
@@ -1666,35 +1688,60 @@ font: 57 12pt &quot;Ubuntu&quot;;</string>
      <widget class="QWidget" name="verticalLayoutWidget">
       <property name="geometry">
        <rect>
-        <x>110</x>
+        <x>150</x>
         <y>10</y>
-        <width>741</width>
+        <width>701</width>
         <height>481</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_rviz"/>
      </widget>
-     <widget class="QWidget" name="">
+     <widget class="QWidget" name="layoutWidget">
       <property name="geometry">
        <rect>
         <x>10</x>
-        <y>9</y>
-        <width>91</width>
-        <height>58</height>
+        <y>400</y>
+        <width>135</width>
+        <height>90</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_10">
        <item>
-        <widget class="QPushButton" name="pushButton">
+        <widget class="QPushButton" name="button_estimate">
+         <property name="styleSheet">
+          <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);
+</string>
+         </property>
          <property name="text">
-          <string>PushButton</string>
+          <string>2D pose Estimate</string>
          </property>
         </widget>
        </item>
        <item>
-        <widget class="QPushButton" name="pushButton_2">
+        <spacer name="horizontalSpacer_2">
+         <property name="orientation">
+          <enum>Qt::Horizontal</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>40</width>
+           <height>20</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+       <item>
+        <widget class="QPushButton" name="button_goal">
+         <property name="styleSheet">
+          <string notr="true">font: 57 12pt &quot;Ubuntu&quot;;
+color: rgb(32, 74, 135);
+border-bottom-color: rgb(186, 189, 182);
+</string>
+         </property>
          <property name="text">
-          <string>PushButton</string>
+          <string>2D Nav Goal</string>
          </property>
         </widget>
        </item>