add xtdrone_qt

control/gcs/develop/xtdrone_qt/CMakeLists.txt

@@ -0,0 +1,59 @@
+##############################################################################
+# CMake
+##############################################################################
+
+cmake_minimum_required(VERSION 2.8.0)
+project(xtdrone_qt)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} "/opt/ros/melodic/")
+##############################################################################
+# Catkin
+##############################################################################
+
+# qt_build provides the qt cmake glue, roscpp the comms for a default talker
+find_package(catkin REQUIRED COMPONENTS qt_build roscpp rviz)
+include_directories(${catkin_INCLUDE_DIRS})
+find_package(Qt5 REQUIRED Core Widgets Charts)
+include_directories(${Qt5_INCLUDE_DIRS})
+#include_directories(${Qt5Charts_INCLUDE_DIRS})
+set(QT_LIBRARIES Qt5::Widgets Qt5::Charts)
+#set(QT_LIBRARIES Qt5::Widgets)
+# Use this to define what the package will export (e.g. libs, headers).
+# Since the default here is to produce only a binary, we don't worry about
+# exporting anything. 
+catkin_package()
+
+##############################################################################
+# Qt Environment
+##############################################################################
+
+# this comes from qt_build's qt-ros.cmake which is automatically 
+# included via the dependency call in package.xml
+#rosbuild_prepare_qt4(QtCore QtGui) # Add the appropriate components to the component list here
+
+##############################################################################
+# Sections
+##############################################################################
+
+file(GLOB QT_FORMS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ui/*.ui)
+file(GLOB QT_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} resources/*.qrc)
+file(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/xtdrone_qt/*.hpp *.h)
+
+QT5_ADD_RESOURCES(QT_RESOURCES_CPP ${QT_RESOURCES})
+QT5_WRAP_UI(QT_FORMS_HPP ${QT_FORMS})
+QT5_WRAP_CPP(QT_MOC_HPP ${QT_MOC})
+
+##############################################################################
+# Sources
+##############################################################################
+
+file(GLOB_RECURSE QT_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS src/*.cpp)
+
+##############################################################################
+# Binaries
+##############################################################################
+
+add_executable(xtdrone_qt ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP})
+target_link_libraries(xtdrone_qt ${QT_LIBRARIES} ${catkin_LIBRARIES})
+install(TARGETS xtdrone_qt RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+

control/gcs/develop/xtdrone_qt/include/xtdrone_qt/form.h

@@ -0,0 +1,29 @@
+#ifndef xtdrone_qt_FORM_H
+#define xtdrone_qt_FORM_H
+
+#include <QWidget>
+
+namespace Ui {
+class Form;
+}
+
+class Form : public QWidget
+{
+    Q_OBJECT
+
+public:
+    explicit Form(QWidget *parent = nullptr);
+    ~Form();
+
+private slots:
+    void on_button_reset_clicked(bool);
+    void on_button_ignore_clicked();
+
+signals:
+    void turn_to_main(bool);
+
+private:
+    Ui::Form *ui;
+};
+
+#endif // FORM_H

control/gcs/develop/xtdrone_qt/include/xtdrone_qt/main_window.hpp

@@ -0,0 +1,128 @@
+/**
+ * @file /include/xtdrone_qt/main_window.hpp
+ *
+ * @brief Qt based gui for xtdrone_qt.
+ *
+ * @date November 2010
+ **/
+#ifndef xtdrone_qt_MAIN_WINDOW_H
+#define xtdrone_qt_MAIN_WINDOW_H
+
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#include <QtWidgets/QMainWindow>
+#include "ui_main_window.h"
+#include "qnode.hpp"
+#include "form.h"
+#include "qrviz.hpp"
+#include <QtCharts/QChart>
+#include <QtCharts/QChartView>
+#include <QtCharts/QLineSeries>
+#include <QtCharts/QValueAxis>
+#include <QProcess>
+#include <QColor>
+#include <QComboBox>
+
+/*****************************************************************************
+** Namespace
+*****************************************************************************/
+
+namespace xtdrone_qt {
+
+/*****************************************************************************
+** Interface [MainWindow]
+*****************************************************************************/
+/**
+ * @brief Qt central, all operations relating to the view part here.
+ */
+class MainWindow : public QMainWindow {
+Q_OBJECT
+
+
+public:
+    MainWindow(int argc, char** argv, QWidget *parent = 0);
+//    MainWindow(LISTINT multirotor_select, int *multirotor_num, QString *multirotor_type QWidget *parent = 0);
+    ~MainWindow();
+    static const int multi_type_num = 8;
+    QString multirotor_type[multi_type_num] = {"iris", "typhoon_h480", "solo", "tailsitter", "quadplane", "plane", "tiltrotor", "ugv"};
+    int multirotor_num[multi_type_num] = {1, 0, 0, 0, 0, 0, 0, 0};
+    double mapXY[multi_type_num][4] = {{-20.0, 21.5, -13.3, 10.7},{-10.5, 17.2, -12.4, 3.5},{-11.54, 21.34, -2.2, 16.8},{-48, 152, -57.09, 59.09}, {-2184, 2980, -1500, 1500},{-1192.9, 648.9, -375, 695},{-48, 152, -57.09, 59.09}, {-100,100,-100,100}};
+    int multirotor_formation_num = 0;
+    QString control_type = "vel";
+    QString task = "Flying";
+    QString map = "indoor1";
+    QString formation2 = "formation1";
+    QString cmd2 = "controll all";
+    int once_flag = 0;
+    int twice_flag = 0;
+    typedef QList<int> LISTINT;
+    typedef QList<std::string> LISTSTR;
+    LISTINT multirotor_select;
+    LISTINT multirotor_get_control;
+    LISTSTR multi_type;
+    int multi_num;
+    QProcess *launch_cmd;
+    bool ctrl_leader;
+    bool cmd_vel_mask;
+    QString formation_last;
+    int ViewW = 882; //(1/2.5)
+    int ViewH = 513; //(1/2.5)
+    double widgetW;
+    double widgetH;
+//    typedef QList<QtCharts::QLineSeries*> LISTSERIES;
+//    LISTSERIES seriesList;
+    std::vector<QColor> color_plot;
+    QtCharts::QLineSeries *series = new QtCharts::QLineSeries();
+    QtCharts::QLineSeries* seriesList[10];
+    QtCharts::QChart* chart = new QtCharts::QChart();
+    QtCharts::QValueAxis *axisX = new QtCharts::QValueAxis;
+    QtCharts::QValueAxis *axisY = new QtCharts::QValueAxis;
+    QtCharts::QChartView* chartview = new QtCharts::QChartView();
+    QPointF TopLeft;
+    QSize size_tab;
+    double zoom;
+    int plot_count = 0;
+    int randnum;
+    QColor color;
+
+
+    void ReadSettings(); // Load up qt program settings at startup
+    void WriteSettings(); // Save qt program settings when closing
+    void Seriesint(int);
+    void closeEvent(QCloseEvent *event); // Overloaded function
+    void showNoMasterMessage();
+
+public Q_SLOTS:
+    /******************************************
+    ** Auto-connections (connectSlotsByName())
+    *******************************************/
+//    void on_button_connect_clicked(bool check );
+    void slot_btn_run_click(bool);
+    void slot_btn_control_click(bool);
+    void slot_btn_connect_click(bool);
+//    void slot_box_go_and_back_2_valuechange(double);
+//    void slot_box_up_and_down_2_valuechange(double);
+//    void slot_box_left_and_right_2_valuechange(double);
+    void slot_box_valuechange(double);
+    void box_command_valuechange(const QString);
+    QString box_formation_valuechange(const QString);
+    //void slot_box_maps_valuechange(const QString);
+    void slot_checkbox_get_uav_control(int);
+    void slot_init(bool);
+    void updateLoggingView(); // no idea why this can't connect automatically
+    void slot_quick_output();
+    void slot_box_maps_change(const QString);
+    void slot_update_plot(float, float);
+
+private:
+    void init_uisettings();
+    Ui::MainWindowDesign ui;
+    QNode qnode;
+    qrviz* my_rviz;
+};
+
+}  // namespace xtdrone_qt
+
+#endif // xtdrone_qt_MAIN_WINDOW_H

control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qnode.hpp

@@ -0,0 +1,124 @@
+/**
+ * @file /include/xtdrone_qt/qnode.hpp
+ *
+ * @brief Communications central!
+ *
+ * @date February 2011
+ **/
+/*****************************************************************************
+** Ifdefs
+*****************************************************************************/
+
+#ifndef xtdrone_qt_QNODE_HPP_
+#define xtdrone_qt_QNODE_HPP_
+
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+// To workaround boost/qt4 problems that won't be bugfixed. Refer to
+//    https://bugreports.qt.io/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <ros/ros.h>
+#endif
+#include <string>
+#include <QThread>
+#include <QStringListModel>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/String.h>
+//#include <boost/bind.hpp>
+#include <vector>
+
+
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace xtdrone_qt {
+
+/*****************************************************************************
+** Class
+*****************************************************************************/
+
+class QNode : public QThread {
+    Q_OBJECT
+public:
+    QNode(int argc, char** argv);
+//    QNode(LISTINT multi_select, int *multi_num, QString *multi_type);
+    virtual ~QNode();
+    typedef QList<int> LISTINT;
+    typedef QList<std::string> LISTSTR;
+    typedef QList<geometry_msgs::Twist> LISTTWIST;
+    typedef QList<std_msgs::String> LISTSTRR;
+//	bool init();
+//  init varibles
+    LISTTWIST vel;
+    LISTSTRR cmd;
+    int multirotor_num = 0;
+    LISTINT multirotor_get_control;
+    LISTSTR multirotor_type;
+    geometry_msgs::Pose local_pose[30];
+    int * test;
+    bool stop_flag;
+    bool start_control_flag;
+    bool ctrl_leader;
+    bool cmd_vel_mask;
+    bool cmd_change_flag;
+    bool cmd_buffer;
+    std::string control_type;
+    // init functions
+    bool init(const LISTINT multi_select, const int *multi_num, const LISTSTR multi_type, std::string control_type);
+//	bool init(const std::string &master_url, const std::string &host_url);
+    void set_cmd(double q_forward, double q_upward, double q_leftward, double q_orientation, bool q_ctrl_leader, std::string &q_cmd, bool q_cmd_vel_mask);
+    void show_log();
+    void start_control(bool q_start_control_flag);
+    void stop_control(bool q_stop_flag);
+    void get_uav_control(LISTINT multirotor_get_control);
+    void run();
+
+
+
+    /*********************
+    ** Logging
+    **********************/
+    enum LogLevel {
+             Debug,
+             Info,
+             Warn,
+             Error,
+             Fatal
+     };
+
+    QStringListModel* loggingModel() { return &logging_model; }
+    void log( const LogLevel &level, const std::string &msg);
+
+Q_SIGNALS:
+    void loggingUpdated();
+    void rosShutdown();
+    void uavposition(float posi_x, float posi_y);
+
+private:
+    int init_argc;
+    char** init_argv;
+    ros::Publisher chatter_publisher;
+//    ros::Publisher chatter_publisher_2;
+    QStringListModel logging_model;
+    std::vector<ros::Subscriber> buffer_multi_odom_groundtruth_sub;
+    std::vector<ros::Subscriber> buffer_local_vel_sub;
+    std::vector<ros::Publisher> buffer_multi_cmd_vel_flu_pub;
+    std::vector<ros::Publisher> buffer_multi_cmd_pub;
+    std::vector<ros::Publisher> buffer_multi_cmd_accel_flu_pub;
+    ros::Publisher leader_cmd_vel_flu_pub;
+    ros::Publisher leader_cmd_accel_flu_pub;
+    ros::Publisher leader_cmd_pub;
+    void publish();
+
+};
+
+}  // namespace xtdrone_qt
+
+#endif /* xtdrone_qt_QNODE_HPP_ */

control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qrviz.hpp

@@ -0,0 +1,21 @@
+#ifndef QRVIZ_HPP
+#define QRVIZ_HPP
+
+#include <QObject>
+#include <rviz/visualization_manager.h>
+#include <rviz/render_panel.h>
+#include <rviz/display.h>
+#include <rviz/tool_manager.h>
+#include <rviz/tool.h>
+#include <ros/ros.h>
+#include <QVBoxLayout>
+
+class qrviz
+{
+public:
+    qrviz(QVBoxLayout* layout);
+private:
+    rviz::RenderPanel* render_panel;
+};
+
+#endif // QRVIZ_HPP

control/gcs/develop/xtdrone_qt/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b xtdrone_qt is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

control/gcs/develop/xtdrone_qt/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>xtdrone_qt</name>
+  <version>0.1.0</version>
+  <description>
+
+     xtdrone_qt
+
+  </description>
+  <maintainer email="robin@gmail.com">robin</maintainer>
+  <author>robin</author>
+  <license>BSD</license>
+  <!-- <url type="bugtracker">https://github.com/stonier/qt_ros/issues</url> -->
+  <!-- <url type="repository">https://github.com/stonier/qt_ros</url> -->
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>qt_build</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>libqt4-dev</build_depend>
+  <build_depend>rviz</build_depend>
+  <run_depend>qt_build</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>libqt4-dev</run_depend>
+  <run_depend>rviz</run_depend>
+
+ 
+</package>

control/gcs/develop/xtdrone_qt/resources/images.qrc

@@ -0,0 +1,12 @@
+<RCC>
+    <qresource prefix="/">
+        <file>images/indoor1.jpg</file>
+        <file>images/indoor2.jpg</file>
+        <file>images/indoor3.jpg</file>
+        <file>images/outdoor2.jpg</file>
+        <file>images/outdoor3.jpg</file>
+        <file>images/robocup.jpg</file>
+        <file>images/xtd.ico</file>
+        <file>images/outdoor1.jpg</file>
+    </qresource>
+</RCC>

control/gcs/develop/xtdrone_qt/src/form.cpp

@@ -0,0 +1,25 @@
+#include "../include/xtdrone_qt/form.h"
+#include "ui_form.h"
+
+Form::Form(QWidget *parent) :
+    QWidget(parent),
+    ui(new Ui::Form)
+{
+    ui->setupUi(this);
+}
+
+Form::~Form()
+{
+    delete ui;
+}
+
+void Form::on_button_reset_clicked(bool)
+{
+    emit turn_to_main(true);
+    this->hide();
+}
+
+void Form::on_button_ignore_clicked()
+{
+    this->hide();
+}

control/gcs/develop/xtdrone_qt/src/image.qrc

@@ -0,0 +1,2 @@
+<!DOCTYPE RCC>
+<RCC version="1.0"/>

control/gcs/develop/xtdrone_qt/src/main.cpp

@@ -0,0 +1,32 @@
+/**
+ * @file /src/main.cpp
+ *
+ * @brief Qt based gui.
+ *
+ * @date November 2010
+ **/
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#include <QtGui>
+#include <QApplication>
+#include "../include/xtdrone_qt/main_window.hpp"
+
+/*****************************************************************************
+** Main
+*****************************************************************************/
+
+int main(int argc, char **argv) {
+
+    /*********************
+    ** Qt
+    **********************/
+    QApplication app(argc, argv);
+    xtdrone_qt::MainWindow w(argc,argv);
+    w.show();
+    app.connect(&app, SIGNAL(lastWindowClosed()), &app, SLOT(quit()));
+    int result = app.exec();
+
+	return result;
+}

control/gcs/develop/xtdrone_qt/src/main_window.cpp

@@ -0,0 +1,745 @@
+/**
+ * @file /src/main_window.cpp
+ *
+ * @brief Implementation for the qt gui.
+ *
+ * @date February 2011
+ **/
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#include <QtGui>
+#include <QMessageBox>
+#include <iostream>
+#include "../include/xtdrone_qt/main_window.hpp"
+#include "QDebug"
+//#include <sstream>
+//#include <string>
+#include <QColor>
+
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace xtdrone_qt {
+
+using namespace Qt;
+
+/*****************************************************************************
+** Implementation [MainWindow]
+*****************************************************************************/
+
+MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
+    : QMainWindow(parent)
+    , qnode(argc, argv)
+//MainWindow::MainWindow(LISTINT multi_select, int *multi_num, QString *multi_type, QWidget *parent)
+//    : QMainWindow(parent)
+//    , qnode(multirotor_select, *multirotor_num, *multirotor_type)
+{
+    ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.
+//    QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); // qApp is a global variable for the application
+
+    ReadSettings();
+    init_uisettings();
+    setWindowIcon(QIcon("://images/xtd.ico"));
+//    ui.tab_manager->setCurrentIndex(0); // ensure the first tab is showing - qt-designer should have this already hardwired, but often loses it (settings?).
+    QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close()));
+    /*********************
+    ** Logging
+    **********************/
+    ui.view_logging->setModel(qnode.loggingModel());
+    QObject::connect(&qnode, SIGNAL(loggingUpdated()), this, SLOT(updateLoggingView()));
+    ui.show_widget_2->setCurrentIndex(0);
+
+    /*********************
+    ** QChart
+    **********************/
+    axisX->setRange(mapXY[0][0], mapXY[0][1]);
+    axisX->setLabelsVisible(false);
+    axisX->setGridLineVisible(false);
+    axisX->setVisible(false);
+    axisY->setRange(mapXY[0][2], mapXY[0][3]);
+    axisY->setLabelsVisible(false);
+    axisY->setGridLineVisible(false);
+    axisY->setVisible(false);
+    for (int i = 0; i < 10; i++)
+        Seriesint(i);
+    size_tab = ui.launch_info_text->size();
+//    chart->createDefaultAxes();
+    chart->legend()->hide();
+    chart->setContentsMargins(0,0,0,0);
+    chart->setMargins(QMargins(0,0,0,0));
+    chart->setBackgroundRoundness(0);
+    chartview->setChart(chart);
+    chart->resize(size_tab);
+    chartview->setRenderHint(QPainter::Antialiasing, true);
+    ui.show_widget->addTab(chartview, "world");
+    chartview->resize(size_tab);
+    chartview->setContentsMargins(0,0,0,0);
+    //settings of background
+    widgetH = static_cast<int>(chart->plotArea().height());
+    widgetW = static_cast<int>(chart->plotArea().width());
+    qDebug()<<"widgetH"<<widgetH;
+    qDebug()<<"widgetW"<<widgetW;
+
+    QImage background("://images/indoor1.jpg");
+    QImage background2 = background.scaled(widgetW,widgetH,Qt::IgnoreAspectRatio,Qt::SmoothTransformation);
+    QImage translated(size_tab.width(), size_tab.height(), QImage::Format_ARGB32);
+//    translated.fill(Qt::white);
+    QPainter painter(&translated);
+//    TopLeft = chart->plotArea().topLeft();
+    TopLeft = chart->plotArea().topLeft();
+    painter.drawImage(TopLeft, background2);
+    chart->setPlotAreaBackgroundBrush(translated);
+    chart->setPlotAreaBackgroundVisible(true);
+    /*********************
+    ** init parameters
+    **********************/
+    ctrl_leader = false;
+    cmd_vel_mask = false;
+    formation_last = "waiting";
+    /*********************
+    ** init Rviz
+    **********************/
+    ui.treeWidget->setWindowTitle("Display");
+//    ui.treeWidget->setHeaderLabels(QStringList()<<"key"<<"value");
+    QTreeWidgetItem* Global = new QTreeWidgetItem(QStringList()<<"Global Options");
+    ui.treeWidget->addTopLevelItem(Global);
+    Global->setExpanded(true);
+    QTreeWidgetItem* Fixed_frame = new QTreeWidgetItem(QStringList()<<"Fixed Frame");
+    QComboBox* fixed_box = new QComboBox();
+    fixed_box->addItem("map");
+    fixed_box->setMaximumWidth(100);
+    fixed_box->setEditable(true);
+    Global->addChild(Fixed_frame);
+    ui.treeWidget->setItemWidget(Fixed_frame,1,fixed_box);
+    ui.treeWidget->header()->setMaximumSectionSize(150);
+    ui.treeWidget->header()->setDefaultSectionSize(150);
+
+}
+
+MainWindow::~MainWindow() {}
+
+void MainWindow::showNoMasterMessage() {
+    QMessageBox msgBox;
+    msgBox.setText("Couldn't find the ros master.");
+    msgBox.exec();
+    close();
+}
+
+/*
+ * These triggers whenever the button is clicked, regardless of whether it
+ * is already checked or not.
+ */
+
+//void MainWindow::on_button_connect_clicked(bool) {
+//    qDebug()<<"init!!!";
+//    if ( !qnode.init() ) {   //toStdString()
+//        showNoMasterMessage();
+//    } else {
+//        ui.button_run->setEnabled(false);
+//    }
+//}
+
+/*****************************************************************************
+** Implemenation [Slots][manually connected]
+*****************************************************************************/
+
+/**
+ * This function is signalled by the underlying model. When the model changes,
+ * this will drop the cursor down to the last line in the QListview to ensure
+ * the user can always see the latest log message.
+ */
+void MainWindow::updateLoggingView() {
+        ui.view_logging->scrollToBottom();
+}
+
+/*****************************************************************************
+** Implementation [Configuration]
+*****************************************************************************/
+
+void MainWindow::ReadSettings() {
+    QSettings settings("Qt-Ros Package", "xtdrone_qt");
+    restoreGeometry(settings.value("geometry").toByteArray());
+    restoreState(settings.value("windowState").toByteArray());
+    connect(ui.button_run,SIGNAL(clicked(bool)),this,SLOT(slot_btn_run_click(bool)));
+    connect(ui.button_stop,SIGNAL(clicked(bool)),this,SLOT(slot_init(bool)));
+    connect(ui.button_control,SIGNAL(clicked(bool)),this,SLOT(slot_btn_control_click(bool)));
+    connect(ui.box_go_and_back_2,SIGNAL(valueChanged(double)),this,SLOT(slot_box_valuechange(double)));
+    connect(ui.box_up_and_down_2,SIGNAL(valueChanged(double)),this,SLOT(slot_box_valuechange(double)));
+    connect(ui.box_left_and_right_2,SIGNAL(valueChanged(double)),this,SLOT(slot_box_valuechange(double)));
+    connect(ui.box_orientation,SIGNAL(valueChanged(double)),this,SLOT(slot_box_valuechange(double)));
+    connect(ui.box_command,SIGNAL(currentTextChanged(const QString)),this,SLOT(box_command_valuechange(const QString)));
+    connect(ui.box_formation,SIGNAL(currentTextChanged(const QString)),this,SLOT(box_command_valuechange(const QString)));
+    //connect(ui.comboBox_maps,SIGNAL(currentIndexChanged(const QString)),this,SLOT(slot_box_maps_valuechange(const QString)));
+    connect(ui.checkBox_iris,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_solo,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_plane,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_rotor,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_typhoon,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_quadplane,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_tiltrotor,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    connect(ui.checkBox_tailsitter,SIGNAL(stateChanged(int)),this,SLOT(slot_checkbox_get_uav_control(int)));
+    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)));
+}
+
+void MainWindow::WriteSettings() {
+    QSettings settings("Qt-Ros Package", "xtdrone_qt");
+    //settings.setValue("topic_name",ui.line_edit_topic->text());
+//    settings.setValue("use_environment_variables",QVariant(ui.checkbox_use_environment->isChecked()));
+    settings.setValue("geometry", saveGeometry());
+    settings.setValue("windowState", saveState());
+//    settings.setValue("remember_settings",QVariant(ui.checkbox_remember_settings->isChecked()));
+
+}
+
+void MainWindow::Seriesint(int ch)
+{
+    seriesList[ch] = new QtCharts::QLineSeries();
+    chart->addSeries(seriesList[ch]);
+    randnum = rand()%(255-16+1)+16;
+    color.setRed(randnum);
+    randnum = rand()%(255-16+1)+16;
+    color.setGreen(randnum);
+    randnum = rand()%(255-16+1)+16;
+    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]);
+}
+
+/*****************************************************************************
+** Implementation [Slots]
+*****************************************************************************/
+
+void MainWindow::init_uisettings()
+{
+    ui.button_run->setChecked(false);
+    ui.button_stop->setChecked(false);
+    ui.button_control->setChecked(false);
+    ui.button_run->setEnabled(true);
+    ui.button_stop->setEnabled(false);
+    ui.button_control->setEnabled(false);
+    ui.box_go_and_back_2->setReadOnly(true);
+    ui.box_up_and_down_2->setReadOnly(true);
+    ui.box_left_and_right_2->setReadOnly(true);
+    ui.box_orientation->setReadOnly(true);
+    ui.box_formation->setVisible(false);
+    ui.checkBox_iris->setCheckable(false);
+    ui.checkBox_solo->setCheckable(false);
+    ui.checkBox_plane->setCheckable(false);
+    ui.checkBox_rotor->setCheckable(false);
+    ui.checkBox_typhoon->setCheckable(false);
+    ui.checkBox_quadplane->setCheckable(false);
+    ui.checkBox_tiltrotor->setCheckable(false);
+    ui.checkBox_tailsitter->setCheckable(false);
+    once_flag = 0;
+    twice_flag = 0;
+    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);
+
+}
+
+void MainWindow::slot_init(bool)
+{
+    qDebug()<<"init?";
+    init_uisettings();
+    qnode.stop_control(true);
+}
+void MainWindow::slot_btn_connect_click(bool)
+{
+    // auto run:
+    QString launch_text = ui.text_launch_2->toPlainText();
+    if (launch_text != "")
+    {
+        launch_cmd = new QProcess;
+        launch_cmd->start("bash");
+        launch_cmd->write(ui.text_launch_2->toPlainText().toLocal8Bit()+'\n');
+        connect(launch_cmd,SIGNAL(readyReadStandardError()),this, SLOT(slot_quick_output()));
+        connect(launch_cmd,SIGNAL(readyReadStandardOutput()),this, SLOT(slot_quick_output()));
+    }
+
+}
+
+void MainWindow::slot_btn_run_click(bool)
+{
+    //self.control_type = str(self.comboBox_controltype.currentText())
+//    Form* f=new Form;
+//    f->show();
+//    connect(f,SIGNAL(turn_to_main(bool)),this,SLOT(slot_init(bool)));
+    once_flag +=1;
+    bool run_flag = true;
+    bool formation_flag = true;
+    map = QString(ui.comboBox_maps->currentText());
+    multirotor_select.clear();
+    multi_type.clear();
+
+    // get setting values of multi-uav
+    if (once_flag==1)
+    {
+        control_type = QString(ui.comboBox_controltype->currentText());
+        for (int i = 0; i < multi_type_num; i++)
+        {
+            if (i == 0)
+            {
+                multirotor_num[i] = int(ui.box_iris_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_iris->setCheckable(true);
+                }
+            }
+            else if (i == 1)
+            {
+                multirotor_num[i] = int(ui.box_typhoon_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_typhoon->setCheckable(true);
+                }
+            }
+            else if (i == 2)
+            {
+                multirotor_num[i] = int(ui.box_solo_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_solo->setCheckable(true);
+                }
+            }
+            else if (i == 3)
+            {
+                multirotor_num[i] = int(ui.box_tailsitter_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_tailsitter->setCheckable(true);
+                }
+            }
+            else if (i == 4)
+            {
+                multirotor_num[i] = int(ui.box_quadplane_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_quadplane->setCheckable(true);
+                }
+            }
+            else if (i == 5)
+            {
+                multirotor_num[i] = int(ui.box_plane_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_plane->setCheckable(true);
+                }
+            }
+            else if (i == 6)
+            {
+                multirotor_num[i] = int(ui.box_tiltrotor_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_tiltrotor->setCheckable(true);
+                }
+            }
+            else
+            {
+                multirotor_num[i] = int(ui.box_rotor_num->value());
+                if (multirotor_num[i] > 0.5)
+                {
+                    multirotor_select.push_back(i);
+                    ui.checkBox_rotor->setCheckable(true);
+                }
+            }
+            if ((multirotor_num[i] == 6) || (multirotor_num[i] == 9) || (multirotor_num[i] == 18))
+            {
+                if (formation_flag)
+                {
+                    ui.box_formation->setVisible(true);
+                    multirotor_formation_num = multirotor_num[i];
+                    formation_flag = false;
+                }
+                else
+                {
+                    run_flag = false;
+                    Form* f=new Form;
+                    f->show();
+                    connect(f,SIGNAL(turn_to_main()),this,SLOT(slot_init()));
+                }
+            }
+
+        }
+        if (run_flag && once_flag==1)
+        {
+            multi_num = 0;
+            int select_num = multirotor_select.size();
+            qDebug()<<"select_num:"<<select_num;
+            for (int j = 0; j < select_num ; j++)
+            {
+                multi_num = multi_num + multirotor_num[multirotor_select[j]];
+                for (int id = 0; id < multirotor_num[multirotor_select[j]] ; id++)
+                    multi_type.push_back(multirotor_type[multirotor_select[j]].toStdString());
+//                qDebug()<<"multirotor_type"<<multi_type[j].c_str();
+            }
+//            qDebug()<<"num:"<<multi_num;
+//            qDebug()<<"multirotor_select"<<multirotor_select;
+//            for (int ii = 0; ii< 8; ii++)
+//                qDebug()<<"multirotor_num"<< multirotor_num[ii];
+            qDebug()<<"init!!!";
+
+            if ( !qnode.init(multirotor_select, multirotor_num, multi_type,control_type.toStdString())) {   //toStdString()
+                showNoMasterMessage();
+            } else {
+                ui.button_run->setEnabled(false);
+                ui.button_control->setEnabled(true);
+                my_rviz = new qrviz(ui.verticalLayout_rviz);
+            }
+        }
+
+    }
+
+    //qDebug()<<multirotor_select;
+    //qDebug()<<multirotor_num[0];
+}
+
+//void MainWindow::slot_btn_stop_click(bool)
+//{
+//    init_uisettings();
+//}
+
+void MainWindow::slot_btn_control_click(bool)
+{
+    twice_flag += 1;
+    qnode.start_control(true);
+    if (twice_flag == 1)
+    {
+        ui.box_go_and_back_2->setReadOnly(false);
+        ui.box_up_and_down_2->setReadOnly(false);
+        ui.box_left_and_right_2->setReadOnly(false);
+        ui.box_orientation->setReadOnly(false);
+        ui.button_stop->setEnabled(true);
+    }
+
+}
+
+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)
+{
+    qDebug()<<"change!!!";
+    double forward_change = ui.box_go_and_back_2->value();
+    double upward_change = ui.box_up_and_down_2->value();
+    double leftward_change = ui.box_left_and_right_2->value();
+    double orientation_change = ui.box_orientation->value();
+    ui.box_command->setCurrentIndex(0);
+    std::string cmd3 = "";
+    cmd3 = cmd2.toStdString();
+    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)
+{
+    qDebug()<<"change!!!";
+    double forward_change = ui.box_go_and_back_2->value();
+    double upward_change = ui.box_up_and_down_2->value();
+    double leftward_change = ui.box_left_and_right_2->value();
+    double orientation_change = ui.box_orientation->value();
+    QString cmd_mid = ui.box_command->currentText();
+    QString formation_mid = ui.box_formation->currentText();
+    if (cmd_mid == "control all")
+    {
+        cmd2 = "";
+        ctrl_leader = false;
+    }
+    else if (cmd_mid == "arm")
+    {
+        cmd2 = "ARM";
+    }
+    else if (cmd_mid == "disarm")
+    {
+        cmd2 = "DISARM";
+    }
+    else if (cmd_mid == "return home")
+    {
+        cmd2 = "AUTO.RTL";
+    }
+    else if (cmd_mid == "take off")
+    {
+        cmd2 = "AUTO.TAKEOFF";
+    }
+    else if (cmd_mid == "land")
+    {
+        cmd2 = "AUTO.LAND";
+    }
+    else if (cmd_mid == "offboard")
+    {
+        cmd2 = "OFFBOARD";
+    }
+    else if (cmd_mid == "hover")
+    {
+        cmd2 = "HOVER";
+        cmd_vel_mask = false;
+        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);
+        forward_change = 0.0;
+        upward_change = 0.0;
+        leftward_change = 0.0;
+        orientation_change = 0.0;
+    }
+    else if (cmd_mid == "control the leader")
+    {
+        cmd2 = "";
+        ctrl_leader = true;
+    }
+    else
+        cmd2 = "";
+
+    if (formation_last != formation_mid)
+    {
+        cmd_vel_mask = true;
+        formation_last = formation_mid;
+        cmd2 = MainWindow::box_formation_valuechange(formation_mid);
+    }
+    std::string cmd3;
+    cmd3 = cmd2.toStdString();
+    qnode.set_cmd(forward_change, upward_change, leftward_change, orientation_change, ctrl_leader, cmd3, cmd_vel_mask);
+}
+
+QString MainWindow::box_formation_valuechange(const QString formation_mid)
+{
+    if (multirotor_formation_num == 6)
+    {
+        if (formation_mid == "formation 1")
+        {
+            formation2 = "T";
+        }
+        else if (formation_mid == "formation 2")
+        {
+            formation2 = "diamond";
+        }
+        else if (formation_mid == "formation 3")
+        {
+            formation2 = "triangle";
+        }
+        else
+        {
+            formation2 = "waiting";
+        }
+    }
+    else if (multirotor_formation_num == 9)
+    {
+        if (formation_mid == "formation 1")
+        {
+            formation2 = "cube";
+        }
+        else if (formation_mid == "formation 2")
+        {
+            formation2 = "pyramid";
+        }
+        else if (formation_mid == "formation 3")
+        {
+            formation2 = "triangle";
+        }
+        else
+        {
+            formation2 = "waiting";
+        }
+    }
+    else
+    {
+        if (formation_mid == "formation 1")
+        {
+            formation2 = "cuboid";
+        }
+        else if (formation_mid == "formation 2")
+        {
+            formation2 = "sphere";
+        }
+        else if (formation_mid == "formation 3")
+        {
+            formation2 = "diamond";
+        }
+        else
+        {
+            formation2 = "waiting";
+        }
+    }
+    return formation2;
+}
+
+void MainWindow::slot_checkbox_get_uav_control(int value)
+{
+    multirotor_get_control.clear();
+    for (int i = 0; i<multirotor_num[0] ;i++)
+    {
+        if (ui.checkBox_iris->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[1] ;i++)
+    {
+        if (ui.checkBox_typhoon->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[2] ;i++)
+    {
+        if (ui.checkBox_solo->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[3] ;i++)
+    {
+        if (ui.checkBox_tailsitter->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[4] ;i++)
+    {
+        if (ui.checkBox_quadplane->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[5] ;i++)
+    {
+        if (ui.checkBox_plane->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[6] ;i++)
+    {
+        if (ui.checkBox_tiltrotor->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    for (int i = 0; i<multirotor_num[7] ;i++)
+    {
+        if (ui.checkBox_rotor->isChecked())
+            multirotor_get_control.push_back(1);
+        else
+            multirotor_get_control.push_back(0);
+    }
+    qDebug()<<"control::::"<<multirotor_get_control;
+    qnode.get_uav_control(multirotor_get_control);
+}
+
+void MainWindow::slot_box_maps_change(const QString value)
+{
+    QString map_address;
+    map = "://images/"+value+".jpg";
+    if (value == "indoor1")
+    {
+        axisX->setRange(mapXY[0][0], mapXY[0][1]);
+        axisY->setRange(mapXY[0][2], mapXY[0][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(-6.1, 0) << QPointF(3.7, -8.4);
+    }
+    else if(value == "indoor2")
+    {
+        axisX->setRange(mapXY[1][0], mapXY[1][1]);
+        axisY->setRange(mapXY[1][2], mapXY[1][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(-6.1, 0) << QPointF(3.7, -8.4);
+    }
+    else if(value == "indoor3")
+    {
+        axisX->setRange(mapXY[2][0], mapXY[2][1]);
+        axisY->setRange(mapXY[2][2], mapXY[2][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(-6.1, 0) << QPointF(3.7, -8.4);
+    }
+    else if(value == "outdoor1")
+    {
+        axisX->setRange(mapXY[3][0], mapXY[3][1]);
+        axisY->setRange(mapXY[3][2], mapXY[3][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(50, -1.2);
+    }
+    else if(value == "outdoor2")
+    {
+        axisX->setRange(mapXY[4][0], mapXY[4][1]);
+        axisY->setRange(mapXY[4][2], mapXY[4][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(-6.1, 0) << QPointF(3.7, -8.4);
+    }
+    else if(value == "outdoor3")
+    {
+        axisX->setRange(mapXY[5][0], mapXY[5][1]);
+        axisY->setRange(mapXY[5][2], mapXY[5][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(-6.1, 0) << QPointF(3.7, -8.4);
+    }
+    else if(value == "robocup")
+    {
+        axisX->setRange(mapXY[6][0], mapXY[6][1]);
+        axisY->setRange(mapXY[6][2], mapXY[6][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(50, -1.2);
+    }
+    else
+    {
+        axisX->setRange(mapXY[7][0], mapXY[7][1]);
+        axisY->setRange(mapXY[7][2], mapXY[7][3]);
+//        chart->removeSeries(series);
+//        series->clear();
+//        *series << QPointF(0, 0) << QPointF(-6.1, 0) << QPointF(3.7, -8.4);
+    }
+    widgetH = static_cast<int>(chart->plotArea().height());
+    widgetW = static_cast<int>(chart->plotArea().width());
+    QImage background(map);
+    QImage background2 = background.scaled(widgetW,widgetH,Qt::IgnoreAspectRatio,Qt::SmoothTransformation);
+//    background2.save("123.jpg");
+//    ui.show_widget->setStyleSheet(R"(QGraphicsView{ background-image:url(123.jpg);})");
+    QImage translated(size_tab.width(), size_tab.height(), QImage::Format_ARGB32);
+    translated.fill(Qt::white);
+    QPainter painter(&translated);
+    painter.drawImage(TopLeft, background2);
+    chart->setPlotAreaBackgroundBrush(translated);
+}
+void MainWindow::closeEvent(QCloseEvent *event)
+{
+    WriteSettings();
+    QMainWindow::closeEvent(event);
+}
+
+void MainWindow::slot_update_plot(float x_position, float y_position)
+{
+//    qDebug()<<"i got it";
+    if (plot_count >= multi_num)
+        plot_count = 0;
+    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;
+//    }
+    plot_count ++;
+}
+}  // namespace xtdrone_qt

control/gcs/develop/xtdrone_qt/src/qnode.cpp

@@ -0,0 +1,360 @@
+/**
+ * @file /src/qnode.cpp
+ *
+ * @brief Ros communication central!
+ *
+ * @date February 2011
+ **/
+
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <string>
+#include <std_msgs/String.h>
+#include <sstream>
+#include "../include/xtdrone_qt/qnode.hpp"
+//#include <boost/bind.hpp>
+#include "QDebug"
+
+typedef QList<geometry_msgs::Twist> LISTTWIST;
+typedef QList<geometry_msgs::Pose> LISTPOSE;
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace xtdrone_qt {
+
+/*****************************************************************************
+** Implementation
+*****************************************************************************/
+
+QNode::QNode(int argc, char** argv) :
+    init_argc(argc),
+    init_argv(argv)
+    {}
+//QNode::QNode(LISTINT multi_select, int *multi_num, QString *multi_type) :
+//    {}
+
+QNode::~QNode() {
+    if(ros::isStarted()) {
+      ros::shutdown(); // explicitly needed since we use ros::start();
+      ros::waitForShutdown();
+    }
+    wait();
+}
+
+/*****************************************************************************
+** callback functions
+*****************************************************************************/
+
+
+void odm_groundtruth_callback(const nav_msgs::Odometry::ConstPtr& msg, int num_odm, geometry_msgs::Pose local_pose[])
+{
+    local_pose[num_odm] = msg->pose.pose;
+//    qDebug()<<"pose::"<<num_odm<<"::"<<local_pose[num_odm].position.y;
+}
+
+/*****************************************************************************
+** main functions
+*****************************************************************************/
+
+//bool QNode::init() {
+//	ros::init("xtdrone_qt");
+//    ros::init(init_argc,init_argv,"xtdrone_qt");
+//	if ( ! ros::master::check() ) {
+//		return false;
+//	}
+//	ros::start(); // explicitly needed since our nodehandle is going out of scope.
+//	ros::NodeHandle n;
+//	// Add your ros communications here.
+//	chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
+
+//	start();
+//	return true;
+//}
+
+bool QNode::init(const LISTINT multi_select, const int *multi_num, const LISTSTR multi_type, std::string q_control_type) {
+//	ros::init("xtdrone_qt");
+    int leng_select = multi_select.size();
+    int counnnt = 0;
+    int counnnnnt = 0;
+    ctrl_leader = false;
+    cmd_vel_mask = false;
+    start_control_flag = false;
+    stop_flag = false;
+    cmd_change_flag = false;
+    control_type = q_control_type;
+    for (int i = 0; i < leng_select; i ++)
+        multirotor_num = multirotor_num + multi_num[multi_select[i]];
+    multirotor_type = multi_type;
+    for (int j = 0; j < multirotor_num; j++)
+        qDebug()<<"multi_type:::"<<multirotor_type[j].c_str();
+
+    qRegisterMetaType<QVector<int>>("QVector<int>");
+    for (int i = 0; i < multirotor_num; i ++)
+    {
+        vel.append(geometry_msgs::Twist());
+        cmd.append(std_msgs::String());
+        multirotor_get_control.append(0);
+    }
+    for (int i = 0; i < multirotor_num; i ++)
+    {
+        vel[i].angular.x = 0.0;
+        vel[i].angular.y = 0.0;
+        cmd[i].data = "";
+    }
+    ros::init(init_argc,init_argv,"xtdrone_qt");
+    if ( ! ros::master::check() ) {
+        return false;
+    }
+    ros::start(); // explicitly needed since our nodehandle is going out of scope.
+    ros::NodeHandle n;
+
+    // Add your ros communications here.
+    chatter_publisher = n.advertise<std_msgs::String>("chatter_0", 1000);
+    if (control_type == "vel")
+    {
+        for (int i = 0; i < leng_select; i ++)
+        {
+            for (int k = 0; k< multi_num[multi_select[i]]; k++)
+            {
+//                qDebug()<<k;
+                if (multi_select[i] == 7)
+                {
+                    buffer_multi_cmd_vel_flu_pub.push_back(n.advertise<geometry_msgs::Twist>("/ugv_"+std::to_string(k)+"/cmd_vel", 1000));
+                    buffer_multi_cmd_pub.push_back(n.advertise<std_msgs::String>("/ugv"+std::to_string(k)+"/cmd",1000));
+                }
+                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_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++;
+                }
+            }
+        }
+        leader_cmd_vel_flu_pub = n.advertise<geometry_msgs::Twist>("/xtdrone/leader/cmd_vel_flu",1000);
+        leader_cmd_pub = n.advertise<std_msgs::String>("/xtdrone/leader/cmd",1000);
+    }
+    else
+    {
+        counnnnnt = 0;
+        for (int i = 0; i < leng_select; i ++)
+        {
+            for (int k = 0; k< multi_num[multi_select[i]]; k++)
+            {
+//                qDebug()<<k;
+                buffer_multi_cmd_accel_flu_pub.push_back(n.advertise<geometry_msgs::Twist>("/xtdrone/"+multi_type[counnnnnt]+"_"+std::to_string(k)+"/cmd_accel_flu", 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++;
+            }
+        }
+        leader_cmd_accel_flu_pub = n.advertise<geometry_msgs::Twist>("/xtdrone/leader/cmd_accel_flu",1000);
+        leader_cmd_pub = n.advertise<std_msgs::String>("/xtdrone/leader/cmd",1000);
+    }
+
+    for (int i = 0; i < leng_select; i ++)
+    {
+        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)));
+            counnnt ++;
+        }
+    }
+
+    start();
+//    qDebug()<<"size"<<sizee;
+
+    return true;
+}
+
+// functions for messages transmit from ui to qnode:
+void QNode::set_cmd(double q_forward, double q_upward, double q_leftward, double q_orientation, bool q_ctrl_leader, std::string &q_cmd, bool q_cmd_vel_mask)
+{
+//    int leng = sizeof(multi_num)/sizeof(int);
+    for (int i = 0; i < multirotor_num; i++)
+    {
+//        qDebug()<<"multi_num:"<<multi_num[i];
+//        qDebug()<<"multi_control:"<<multirotor_get_control;
+        if (multirotor_get_control[i])
+        {
+            vel[i].linear.x = q_forward;
+            vel[i].linear.y = q_leftward;
+            vel[i].linear.z = q_upward;
+            vel[i].angular.z = q_orientation;
+            cmd[i].data = q_cmd;
+        }
+    }
+    ctrl_leader = q_ctrl_leader;
+    cmd_vel_mask = q_cmd_vel_mask;
+    cmd_change_flag = true;
+//    qDebug()<<"forward:"<<q_forward;
+//    qDebug()<<"upward:"<<q_upward;
+//    qDebug()<<"leftward:"<<q_leftward;
+//    qDebug()<<"orientation:"<<q_orientation;
+//    qDebug()<<"cmd:"<<QString::fromStdString(cmd[0].data);
+//    ROS_INFO_STREAM(q_cmd);
+
+}
+void QNode::get_uav_control(LISTINT multirotor_control)
+{
+    multirotor_get_control = multirotor_control;
+    qDebug()<<"multi_control"<< multirotor_get_control;
+}
+void QNode::stop_control(bool q_stop_flag)
+{
+    stop_flag = q_stop_flag;
+    start_control_flag = false;
+    qDebug()<<"stop!!!";
+}
+void QNode::start_control(bool q_start_control_flag)
+{
+    start_control_flag = q_start_control_flag;
+    stop_flag = false;
+    qDebug()<<"control!!!";
+}
+
+// functions of ROS
+void QNode::run() {
+    ros::Rate loop_rate(100);
+    int count = 0;
+    std::string text_all;
+    std::string text[multirotor_num];
+    std_msgs::String msg;
+//    geometry_msgs::Twist twist[multirotor_num];
+    qDebug()<<"start!!!";
+    while ( ros::ok() ) {
+        // show log:
+        text_all = "";
+        if (count%30 == 0)
+        {
+            if (multirotor_num < 10)
+            {
+                for (int id = 0; id <multirotor_num; id++)
+                {
+                    text[id] = multirotor_type[id] + " pose:\n" + "uav" + std::to_string(id) + ":\n" + "x:" + std::to_string(local_pose[id].position.x)\
+                            + "   y:" + std::to_string(local_pose[id].position.y) + "   z:" + std::to_string(local_pose[id].position.z) + "\n";
+                    text_all = text_all + text[id];
+                    if (count > 30)
+                        emit uavposition(local_pose[id].position.x, local_pose[id].position.y);
+                }
+                msg.data = text_all;
+                chatter_publisher.publish(msg);
+//                chatter_publisher_2.publish(msg);
+                log(Info,msg.data);
+            }
+        }
+        if (start_control_flag)
+        {
+            // update cmd:
+            cmd_buffer = false;
+//            qDebug()<<"cmd_change_flag"<<cmd_change_flag;
+            for (int i = 0; i < multirotor_num; i ++)
+            {
+                if (!cmd_change_flag)
+                {
+                    cmd_buffer = true;
+                    cmd[i].data = "";
+                }
+                if (cmd[i].data != "")
+                {
+                    qDebug()<<"cmd222:"<<QString::fromStdString(cmd[i].data);
+                    qDebug()<<"forward2:"<<vel[i].linear.x;
+                    qDebug()<<"upward2:"<<vel[i].linear.z;
+                    qDebug()<<"leftward2:"<<vel[i].linear.y;
+                    qDebug()<<"orientation2:"<<vel[i].angular.z;
+                }
+            }
+            cmd_change_flag = false;
+            QNode::publish();
+        }
+        if (stop_flag)
+        {
+            for (int i = 0; i < multirotor_num; 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;
+                cmd[i].data = "AUTO.RTL";
+            }
+            QNode::publish();
+            stop_flag = false;
+            break;
+        }
+        ros::spinOnce();
+        loop_rate.sleep();
+        ++count;
+    }
+    qDebug()<<"finish!";
+    std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
+    Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
+}
+void QNode::publish()
+{
+    if (ctrl_leader)
+    {
+        if (control_type == "vel")
+            leader_cmd_vel_flu_pub.publish(vel[1]);
+        else
+            leader_cmd_accel_flu_pub.publish(vel[1]);
+        leader_cmd_pub.publish(cmd[1]);
+    }
+    else
+    {
+        for(int i = 0; i < multirotor_num; i++)
+        {
+            if(!cmd_vel_mask)
+            {
+                if (control_type == "vel")
+                    buffer_multi_cmd_vel_flu_pub[i].publish(vel[i]);
+                else
+                    buffer_multi_cmd_accel_flu_pub[i].publish(vel[i]);
+                buffer_multi_cmd_pub[i].publish(cmd[i]);
+//                qDebug()<<"cmd333333:"<<QString::fromStdString(cmd[i].data);
+            }
+        }
+    }
+}
+
+void QNode::log( const LogLevel &level, const std::string &msg) {
+    logging_model.insertRows(logging_model.rowCount(),1);
+    std::stringstream logging_model_msg;
+    switch ( level ) {
+        case(Debug) : {
+                ROS_DEBUG_STREAM(msg);
+                logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
+                break;
+        }
+        case(Info) : {
+//				ROS_INFO_STREAM(msg);
+//				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
+                logging_model_msg << msg;
+                break;
+        }
+        case(Warn) : {
+                ROS_WARN_STREAM(msg);
+                logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
+                break;
+        }
+        case(Error) : {
+                ROS_ERROR_STREAM(msg);
+                logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
+                break;
+        }
+        case(Fatal) : {
+                ROS_FATAL_STREAM(msg);
+                logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
+                break;
+        }
+    }
+    QVariant new_row(QString(logging_model_msg.str().c_str()));
+    logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
+    Q_EMIT loggingUpdated(); // used to readjust the scrollbar
+}
+
+}  // namespace xtdrone_qt

control/gcs/develop/xtdrone_qt/src/qrviz.cpp

@@ -0,0 +1,8 @@
+#include "../include/xtdrone_qt/qrviz.hpp"
+
+qrviz::qrviz(QVBoxLayout* layout)
+{
+    // create rviz
+    render_panel = new rviz::RenderPanel();
+    layout->addWidget(render_panel);
+}

control/gcs/develop/xtdrone_qt/ui/form.ui

@@ -0,0 +1,72 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <widget class="QTextBrowser" name="textBrowser">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>20</y>
+     <width>351</width>
+     <height>211</height>
+    </rect>
+   </property>
+   <property name="html">
+    <string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
+&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
+p, li { white-space: pre-wrap; }
+&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;&quot;&gt;
+&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:16pt; font-weight:600; color:#a40000;&quot;&gt;Warning!&lt;/span&gt;&lt;/p&gt;
+&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu';&quot;&gt;&lt;br /&gt;&lt;/p&gt;
+&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu';&quot;&gt;As the number of formation control of UAVs are 6 or 9 or 18 in XTDrone formation demo., and the number of UAVs of more than one types that you set are 6 or 9 or 18. The formation control may be disordered. &lt;/span&gt;&lt;/p&gt;
+&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu';&quot;&gt;&lt;br /&gt;&lt;/p&gt;
+&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu';&quot;&gt;If you are willing to control UAVs with formation control, you'd better keep only one type. If not, you can ignore this warning.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="button_reset">
+   <property name="geometry">
+    <rect>
+     <x>50</x>
+     <y>250</y>
+     <width>89</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">font: 75 15pt &quot;Ubuntu Mono&quot;;</string>
+   </property>
+   <property name="text">
+    <string>Reset</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="button_ignore">
+   <property name="geometry">
+    <rect>
+     <x>250</x>
+     <y>250</y>
+     <width>89</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">font: 75 15pt &quot;Ubuntu Mono&quot;;</string>
+   </property>
+   <property name="text">
+    <string>Ignore</string>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

coordination/launch_generator/multi_vehicle.launch

@@ -18,8 +18,8 @@
         <arg name="verbose" value="$(arg verbose)"/>
         <arg name="paused" value="$(arg paused)"/>
     </include>
-     <!-- iris_0 -->
+     <!-- typhoon_h480_0 -->
-     <group ns="iris_0">
+     <group ns="typhoon_h480_0">
         <!-- MAVROS and vehicle configs -->
             <arg name="ID" value="0"/>
             <arg name="ID_in_group" value="0"/>
@@ -32,8 +32,8 @@
             <arg name="R" value="0"/>
             <arg name="P" value="0"/>
             <arg name="Y" value="0"/>
-            <arg name="vehicle" value="iris"/>
+            <arg name="vehicle" value="typhoon_h480"/>
-            <arg name="sdf" value="iris"/>
+            <arg name="sdf" value="typhoon_h480"/>
             <arg name="mavlink_udp_port" value="18570"/>
             <arg name="mavlink_tcp_port" value="4560"/>
             <arg name="ID" value="$(arg ID)"/>
@@ -46,25 +46,112 @@
             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
             <arg name="tgt_component" value="1"/>
         </include>
+    </group>
+     <!-- typhoon_h480_1 -->
+     <group ns="typhoon_h480_1">
+        <!-- MAVROS and vehicle configs -->
+            <arg name="ID" value="1"/>
+            <arg name="ID_in_group" value="1"/>
+            <arg name="fcu_url" default="udp://:24541@localhost:34581"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
+            <arg name="x" value="0"/>
+            <arg name="y" value="6"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="typhoon_h480"/>
+            <arg name="sdf" value="typhoon_h480"/>
+            <arg name="mavlink_udp_port" value="18571"/>
+            <arg name="mavlink_tcp_port" value="4561"/>
+            <arg name="ID" value="$(arg ID)"/>
+            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+     <!-- iris_0 -->
+     <group ns="iris_0">
+        <!-- MAVROS and vehicle configs -->
+            <arg name="ID" value="2"/>
+            <arg name="ID_in_group" value="0"/>
+            <arg name="fcu_url" default="udp://:24542@localhost:34582"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
+            <arg name="x" value="3"/>
+            <arg name="y" value="3"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="iris"/>
+            <arg name="sdf" value="iris_stereo_camera"/>
+            <arg name="mavlink_udp_port" value="18572"/>
+            <arg name="mavlink_tcp_port" value="4562"/>
+            <arg name="ID" value="$(arg ID)"/>
+            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
     </group>
      <!-- iris_1 -->
      <group ns="iris_1">
         <!-- MAVROS and vehicle configs -->
-            <arg name="ID" value="1"/>
+            <arg name="ID" value="3"/>
             <arg name="ID_in_group" value="1"/>
-            <arg name="fcu_url" default="udp://:24541@localhost:34581"/>
+            <arg name="fcu_url" default="udp://:24543@localhost:34583"/>
         <!-- PX4 SITL and vehicle spawn -->
         <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
-            <arg name="x" value="0"/>
+            <arg name="x" value="3"/>
             <arg name="y" value="6"/>
             <arg name="z" value="0"/>
             <arg name="R" value="0"/>
             <arg name="P" value="0"/>
             <arg name="Y" value="0"/>
             <arg name="vehicle" value="iris"/>
-            <arg name="sdf" value="iris"/>
+            <arg name="sdf" value="iris_stereo_camera"/>
-            <arg name="mavlink_udp_port" value="18571"/>
+            <arg name="mavlink_udp_port" value="18573"/>
-            <arg name="mavlink_tcp_port" value="4561"/>
+            <arg name="mavlink_tcp_port" value="4563"/>
+            <arg name="ID" value="$(arg ID)"/>
+            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+     <!-- iris_2 -->
+     <group ns="iris_2">
+        <!-- MAVROS and vehicle configs -->
+            <arg name="ID" value="4"/>
+            <arg name="ID_in_group" value="2"/>
+            <arg name="fcu_url" default="udp://:24544@localhost:34584"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
+            <arg name="x" value="3"/>
+            <arg name="y" value="9"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="iris"/>
+            <arg name="sdf" value="iris_stereo_camera"/>
+            <arg name="mavlink_udp_port" value="18574"/>
+            <arg name="mavlink_tcp_port" value="4564"/>
             <arg name="ID" value="$(arg ID)"/>
             <arg name="ID_in_group" value="$(arg ID_in_group)"/>
         </include>