同意合并
pull/32/head
pvqf6mep3 3 years ago
commit 4d814fb263

@ -0,0 +1,55 @@
#ifndef JOYSTICK_H
#define JOYSTICK_H
#include <QDebug>
#include <QDrag>
#include <QMouseEvent>
#include <QPainter>
#include <QTimer>
#include <QWidget>
#include <QtMath>
class JoyStick : public QWidget {
Q_OBJECT
public:
JoyStick(QWidget *parent = 0);
~JoyStick();
enum {
upleft = 0,
up,
upright,
left,
stop,
right,
downleft,
down,
downright
};
signals:
void keyNumchanged(int num);
protected:
void paintEvent(QPaintEvent *event) override;
void mouseMoveEvent(QMouseEvent *event) override;
void mouseReleaseEvent(QMouseEvent *event) override;
void mousePressEvent(QMouseEvent *event) override;
// void resizeEvent(QResizeEvent *event)override;
private:
int mouseX;
int mouseY;
int JoyStickX; //摇杆
int JoyStickY;
int JoyStickR;
int padX; //底盘
int padY;
int padR;
double handPadDis; //两圆圆心距离
bool mousePressed;
QTimer *tim;
private:
double Pointdis(int a, int b, int x, int y); //两点距离
int getKeyNum();
};
#endif // JoyStick_H

@ -37,7 +37,7 @@ private:
char **init_argv;
ros::Subscriber cmdVel_sub;
ros::Publisher KeyboardControl_cmd_pub;
ros::Publisher cmd_pub;
};

@ -0,0 +1,105 @@
#include "../include/joystick.h"
#include <QDebug>
JoyStick::JoyStick(QWidget* parent) : QWidget(parent) {
setPalette(QPalette(Qt::white));
resize(parent->width(), parent->height());
setMinimumSize(100, 100);
mouseX = width() / 2;
mouseY = height() / 2;
tim = new QTimer(this);
connect(tim, &QTimer::timeout, this,
[=] { emit keyNumchanged(getKeyNum()); });
// connect(this,&JoyStick::keyNumchanged,this,[=](int num){
// qDebug()<<num<<endl;
// });
}
JoyStick::~JoyStick() {}
void JoyStick::paintEvent(QPaintEvent*) {
QPainter painter(this);
int side = qMin(width(), height());
padR = side / 2; //底盘半径
padX = padR; //底盘圆心
padY = padR; //底盘圆心
JoyStickR = padR / 4; //摇杆圆半径
int JoyStickMaxR = padR - JoyStickR;
QColor JoyStickColor;
JoyStickColor.setRgb(85, 87, 83);
//加载底盘图像
// painter.save();
// painter.scale(side / 400.0, side / 400.0);//坐标会随窗口缩放
// painter.drawPixmap(0, 0, QPixmap(":/image/pad.png"));
// painter.restore();
//自绘底盘
painter.save();
QRadialGradient RadialGradient(padR, padR, padR * 3, padR,
padR); //圆心2半径1焦点2
RadialGradient.setColorAt(0, QColor(255, 253, 253, 255)); //渐变
RadialGradient.setColorAt(1, QColor(255, 240, 245, 190)); //渐变
painter.setBrush(RadialGradient);
painter.setPen(Qt::NoPen);
painter.drawEllipse(QPoint(padR, padR), side / 2, side / 2); //大圆盘
painter.restore();
// painter.drawText(20,20,tr("%1,%2,%3").arg(mouseX).arg(mouseY).arg(handPadDis));
if (!mousePressed) { //鼠标没按下则摇杆恢复到底盘中心
mouseX = padX;
mouseY = padY;
}
handPadDis = Pointdis(padR, padR, mouseX, mouseY);
if (handPadDis <= JoyStickMaxR) {
JoyStickX = mouseX;
JoyStickY = mouseY;
} else {
JoyStickX = (int)(JoyStickMaxR * (mouseX - padX) / handPadDis + padX);
JoyStickY = (int)(JoyStickMaxR * (mouseY - padY) / handPadDis + padY);
}
// painter.drawText(200,200,tr("%1,%2,%3").arg(JoyStickX).arg(JoyStickY).arg(handPaddis));
painter.setPen(Qt::NoPen);
painter.setBrush(JoyStickColor);
painter.drawEllipse(QPoint(JoyStickX, JoyStickY), JoyStickR,
JoyStickR); //摇杆
}
void JoyStick::mouseMoveEvent(QMouseEvent* event) {
static bool r = false;
mouseX = event->pos().x();
mouseY = event->pos().y();
if (r == true) {
update();
r = false;
} else {
r = true;
}
}
void JoyStick::mouseReleaseEvent(QMouseEvent* event) {
mouseX = width() / 2;
mouseY = height() / 2;
tim->stop();
mousePressed = false;
emit keyNumchanged(JoyStick::stop);
update();
}
void JoyStick::mousePressEvent(QMouseEvent* event) {
mouseX = event->pos().x();
mouseY = event->pos().y();
tim->start(100);
mousePressed = true;
update();
}
double JoyStick::Pointdis(int a, int b, int x, int y) {
return sqrt((double)((x - a) * (x - a) + (y - b) * (y - b)));
}
int JoyStick::getKeyNum() {
int x, y;
int keynum;
x = (int)(JoyStickX * 3.0 / (padR * 2));
y = (int)(JoyStickY * 3.0 / (padR * 2));
keynum = 3 * y + x;
return keynum;
}

@ -9,7 +9,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
{
qnode.init();
ui->setupUi(this);
setBtnStates();
initUis();
connections();
}
@ -19,41 +19,12 @@ MainWindow::~MainWindow()
}
void MainWindow::initUis(){
/*
* set PushButton state
*/
void MainWindow::slot_keyboard_control(){
QPushButton *btn = qobject_cast<QPushButton *>(sender());
std::string btn_name = btn->text().toStdString();
char key = ' ';
if (btn_name == "")
key = '1';
else if (btn_name == "")
key = '2';
else if (btn_name == "")
key = '3';
else if (btn_name == "")
key = '4';
//else if (btn_name == "")
// key = '5';
else if (btn_name == "")
key = '6';
else if (btn_name == "")
key = '7';
else if (btn_name == "")
key = '8';
else if (btn_name == "")
key = '9';
//速度
float liner = ui->horizontalSlider_linear->value() * 0.01;
float turn = ui->horizontalSlider_raw->value() * 0.01;
//bool is_rage_mode = ui->checkBox_rage_mode->isChecked();
std::cout<<liner<<" "<<turn<<std::endl;
qnode.KeyboardMove(key, liner, turn);
}
void MainWindow::setBtnStates(){
// 8 -> Forward: ↑
ui->pushButton_Forward->setShortcut(Qt::Key_8);
ui->pushButton_Forward->setStyleSheet(
"QPushButton{border-image: url(://images/up.png)}"
@ -61,67 +32,165 @@ void MainWindow::setBtnStates(){
"QPushButton:pressed{border-image: url(://images/up_2.png)}");
ui->pushButton_Forward->setFlat(true);
// 2 -> Back: ↓
ui->pushButton_Back->setShortcut(Qt::Key_2);
ui->pushButton_Back->setStyleSheet(
"QPushButton{border-image: url(://images/up.png)}"
"QPushButton{border-image: url(://images/down.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/up_2.png)}");
"QPushButton:pressed{border-image: url(://images/down_2.png)}");
ui->pushButton_Back->setFlat(true);
// 4 -> Left: ←
ui->pushButton_Left->setShortcut(Qt::Key_4);
ui->pushButton_Left->setStyleSheet("border:none");
ui->pushButton_Left->setStyleSheet(
"QPushButton{border-image: url(://images/left.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/left_2.png)}");
ui->pushButton_Left->setFlat(true);
// 6 -> Right: →
ui->pushButton_Right->setShortcut(Qt::Key_6);
ui->pushButton_Right->setStyleSheet("border:none");
ui->pushButton_Right->setStyleSheet(
"QPushButton{border-image: url(://images/right.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/right_2.png)}");
ui->pushButton_Right->setFlat(true);
// 7 -> Left_Forward: ↖
ui->pushButton_Left_Forward->setShortcut(Qt::Key_7);
ui->pushButton_Left_Forward->setStyleSheet("border:none");
ui->pushButton_Left_Forward->setStyleSheet(
"QPushButton{border-image: url(://images/up_left.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/up_left_2.png)}");
ui->pushButton_Left_Forward->setFlat(true);
// 9 -> Right_Forward: ↗
ui->pushButton_Right_Forward->setShortcut(Qt::Key_9);
ui->pushButton_Right_Forward->setStyleSheet("border:none");
ui->pushButton_Right_Forward->setStyleSheet(
"QPushButton{border-image: url(://images/up_right.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/up_right_2.png)}");
ui->pushButton_Right_Forward->setFlat(true);
// 1 -> Left_Back: ↙
ui->pushButton_Left_Back->setShortcut(Qt::Key_1);
ui->pushButton_Left_Back->setStyleSheet("border:none");
ui->pushButton_Left_Back->setStyleSheet(
"QPushButton{border-image: url(://images/down_left.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/down_left_2.png)}");
ui->pushButton_Left_Back->setFlat(true);
// 3 -> Right_Back: ↘
ui->pushButton_Right_Back->setShortcut(Qt::Key_3);
ui->pushButton_Right_Back->setStyleSheet("border:none");
ui->pushButton_Right_Back->setStyleSheet(
"QPushButton{border-image: url(://images/down_right.png)}"
"QPushButton{border:none}"
"QPushButton:pressed{border-image: url(://images/down_right_2.png)}");
ui->pushButton_Right_Back->setFlat(true);
}
void MainWindow::connections(){
//绑定速度控制按钮
// 8 -> Forward:↑
/*
* init
*/
connect(ui->pushButton_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
rock_widget = new JoyStick(ui->JoyStick_widget);
rock_widget->show();
connect(ui->pushButton_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
}
connect(ui->pushButton_Left, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
void MainWindow::slot_keyboard_control(){
QPushButton *btn = qobject_cast<QPushButton *>(sender());
std::string btn_name = btn->text().toStdString();
char key = ' ';
if (btn_name == "")
key = '1';
else if (btn_name == "")
key = '2';
else if (btn_name == "")
key = '3';
else if (btn_name == "")
key = '4';
//else if (btn_name == "")
// key = '5';
else if (btn_name == "")
key = '6';
else if (btn_name == "")
key = '7';
else if (btn_name == "")
key = '8';
else if (btn_name == "")
key = '9';
connect(ui->pushButton_Right, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
//速度
float liner = ui->horizontalSlider_linear->value() * 0.01;
float turn = ui->horizontalSlider_raw->value() * 0.01;
//bool is_rage_mode = ui->checkBox_rage_mode->isChecked();
//std::cout<<liner<<" "<<turn<<std::endl;
qnode.KeyboardMove(key, liner, turn);
}
connect(ui->pushButton_Left_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
void MainWindow::slot_rockKeyChange(int key){
//速度
float liner = ui->horizontalSlider_linear->value() * 0.01;
float turn = ui->horizontalSlider_raw->value() * 0.01;
switch (key) {
case upleft:
qnode.KeyboardMove('7', liner, turn);
break;
case up:
qnode.KeyboardMove('8', liner, turn);
break;
case upright:
qnode.KeyboardMove('9', liner, turn);
break;
case left:
qnode.KeyboardMove('4', liner, turn);
break;
case right:
qnode.KeyboardMove('6', liner, turn);
break;
case downleft:
qnode.KeyboardMove('1', liner, turn);
break;
case down:
qnode.KeyboardMove('2', liner, turn);
break;
case downright:
qnode.KeyboardMove('3', liner, turn);
break;
}
connect(ui->pushButton_Right_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
}
void MainWindow::connections(){
/*
*
*/
// 8 -> Forward: ↑
connect(ui->pushButton_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 2 -> Back: ↓
connect(ui->pushButton_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 4 -> Left: ←
connect(ui->pushButton_Left, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 6 -> Right: →
connect(ui->pushButton_Right, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 7 -> Left_Forward: ↖
connect(ui->pushButton_Left_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 9 -> Right_Forward: ↗
connect(ui->pushButton_Right_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 1 -> Left_Back: ↙
connect(ui->pushButton_Left_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
// 3 -> Right_Back: ↘
connect(ui->pushButton_Right_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
connect(rock_widget, SIGNAL(keyNumchanged(int)), this,
SLOT(slot_rockKeyChange(int)));
//绑定slider的函数
connect(ui->horizontalSlider_raw, SIGNAL(valueChanged(int)), this,

@ -27,7 +27,7 @@ bool QNode::init() {
void QNode::SubAndPubTopic(){
ros::NodeHandle n;
KeyboardControl_cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
@ -58,7 +58,7 @@ void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
twist.angular.y = 0;
twist.angular.z = th * turn;
KeyboardControl_cmd_pub.publish(twist);
cmd_pub.publish(twist);
ros::spinOnce();
}

@ -6,6 +6,7 @@
#include <QMutex>
#include "ui_mainwindow.h"
#include "../include/joystick.h"
#include "../include/qnode.hpp"
namespace Ui {
@ -19,12 +20,24 @@ class MainWindow : public QMainWindow
public:
explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
~MainWindow();
enum {
upleft = 0,
up,
upright,
left,
stop,
right,
downleft,
down,
downright
};
public slots:
void slot_keyboard_control();
//void slot_command_control(int linear, int angular);
void slot_rockKeyChange(int);
void Slider_raw_valueChanged(int v);
void Slider_linear_valueChanged(int v);
@ -32,9 +45,12 @@ public slots:
private:
Ui::MainWindow *ui;
void setBtnStates();
void connections();
QNode qnode;
JoyStick *rock_widget;
void initUis();
void connections();
};

@ -20,10 +20,10 @@
<widget class="QWidget" name="layoutWidget_2">
<property name="geometry">
<rect>
<x>510</x>
<y>40</y>
<width>446</width>
<height>543</height>
<x>1045</x>
<y>160</y>
<width>387</width>
<height>566</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
@ -265,41 +265,109 @@ QPushButton:pressed{border-image: url(://images/down_2.png);}</string>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_21">
<item>
<widget class="QLabel" name="label_linear_">
<property name="text">
<string>线速度(cm/s)</string>
</property>
</widget>
</item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QSlider" name="horizontalSlider_linear">
<property name="maximum">
<number>100</number>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="singleStep">
<number>1</number>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="value">
<number>50</number>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_21">
<item>
<widget class="QLabel" name="label_linear_">
<property name="text">
<string>线速度(cm/s)</string>
</property>
</widget>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_linear">
<property name="maximum">
<number>100</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>50</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_linear">
<property name="text">
<string>50</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_linear">
<property name="text">
<string>100</string>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
</widget>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_20">
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="label_raw_">
<property name="text">
@ -327,8 +395,40 @@ QPushButton:pressed{border-image: url(://images/down_2.png);}</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_24">
<item>
@ -336,9 +436,12 @@ QPushButton:pressed{border-image: url(://images/down_2.png);}</string>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Preferred</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<width>86</width>
<height>20</height>
</size>
</property>
@ -360,6 +463,9 @@ QPushButton:pressed{border-image: url(://images/down_2.png);}</string>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Preferred</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
@ -383,48 +489,8 @@ QPushButton:pressed{border-image: url(://images/down_2.png);}</string>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="pushButton">
<property name="text">
<string>🔗断开</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QPushButton" name="pushButton_back">
<property name="geometry">
<rect>
<x>210</x>
<y>430</y>
<width>64</width>
<height>64</height>
</rect>
</property>
<property name="minimumSize">
<size>
<width>64</width>
<height>64</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>64</width>
<height>64</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">QPushButton{border-image: url(://images/down.png);}
QPushButton{border:none;}
QPushButton:pressed{border-image: url(://images/down_2.png);}</string>
</property>
<property name="text">
<string>,</string>
</property>
<property name="shortcut">
<string>,</string>
</property>
</widget>
</widget>
<widget class="QMenuBar" name="menubar">
<property name="geometry">

Loading…
Cancel
Save