jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify xtdrone_qt

This commit is contained in:
robin_shaun 2021-03-30 20:23:13 +08:00
parent 34a944378b
commit 613ef03feb
12 changed files with 856 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
control/gcs/develop/xtdrone_qt/README.md Normal file
View File

29
control/gcs/develop/xtdrone_qt/include/xtdrone_qt/formrviz.h Normal file
View File

@ -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

30
control/gcs/develop/xtdrone_qt/include/xtdrone_qt/main_window.hpp
View File

@ -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;

14
control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qnode.hpp
View File

@ -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);
};

17
control/gcs/develop/xtdrone_qt/include/xtdrone_qt/qrviz.hpp
View File

@ -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

49
control/gcs/develop/xtdrone_qt/src/formrviz.cpp Normal file
View File

@ -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();
}

206
control/gcs/develop/xtdrone_qt/src/main_window.cpp
View File

@ -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

191
control/gcs/develop/xtdrone_qt/src/qnode.cpp
View File

@ -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,8 +383,11 @@ void QNode::run() {
}
}
cmd_change_flag = false;
if (!get_goal_flag)
{
QNode::publish();
}
}
if (stop_flag)
{
for (int i = 0; i < multirotor_num; i++)
@ -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

107
control/gcs/develop/xtdrone_qt/src/qrviz.cpp
View File

@ -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);
}

8
control/gcs/develop/xtdrone_qt/ui/form.ui
View File

@ -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>

160
control/gcs/develop/xtdrone_qt/ui/formrviz.ui Normal file
View File

@ -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>

73
control/gcs/develop/xtdrone_qt/ui/main_window.ui
View File

@ -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>