Code2

MainWindow2.h

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#include <QtWidgets>
#include "ros/ros.h"
#include "turtlesim/Pose.h"

class MainWindow2 : public QWidget {
 public:
  MainWindow2(ros::NodeHandle *node, QWidget *parent = nullptr);
  ~MainWindow2() override;

 private:
  ros::NodeHandle *node;
  ros::Publisher publisher;
  ros::Subscriber subscriber;

  QTimer* updateTimer;

  //布局
  QFormLayout *layout;
  //线速度输入框
  QLineEdit *editLinear;
  //角速度输入框
  QLineEdit *editAngular;
  // 发送按钮
  QPushButton *btnSend;
  // X坐标
  QLabel *labelX;
  // Y坐标
  QLabel *labelY;
  // 当前线速度
  QLabel *labelLinear;
  // 当前角速度
  QLabel *labelAngular;
  // 当前角度
  QLabel *labelDegrees;

 private:
  //初始化控件和布局
  void initWidgets();
  void onUpdate();
  //发送点击事件
  void clickSend();
  //poseCallback
  void poseCallback(const turtlesim::Pose::ConstPtr &message);
};

MainWindow2.cpp

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
#include "MainWindow2.h"
#include <iostream>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include <math.h>

using namespace std;

MainWindow2::MainWindow2(ros::NodeHandle *node, QWidget *parent) : QWidget(parent), node(node) {
  //初始化控件和布局
  initWidgets();
}

MainWindow2::~MainWindow2() = default;

void MainWindow2::initWidgets() {
  updateTimer = new QTimer(this);
  updateTimer->setInterval(16);
  updateTimer->start();

  connect(updateTimer, &QTimer::timeout, this, &MainWindow2::onUpdate);

  //窗体设置
  setWindowTitle("小乌龟控制");
  //设置窗体大小
  resize(400, 120);

  //设置布局
  layout = new QFormLayout();
  setLayout(layout);

  //添加输入框
  editLinear = new QLineEdit();
  layout->addRow("线速度", editLinear);

  //添加输入框
  editAngular = new QLineEdit();
  layout->addRow("角速度", editAngular);

  //添加显示
  labelX = new QLabel();
  layout->addRow("当前X坐标", labelX);

  labelY = new QLabel();
  layout->addRow("当前Y坐标", labelY);

  labelLinear = new QLabel();
  layout->addRow("当前线速度", labelLinear);

  labelAngular = new QLabel();
  layout->addRow("当前角速度", labelAngular);

  labelDegrees = new QLabel();
  layout->addRow("当前角度", labelDegrees);

  //添加按钮
  btnSend = new QPushButton("发送");
  layout->addRow(btnSend);

  //添加事件
  btnSend->connect(btnSend, &QPushButton::clicked, this, &MainWindow2::clickSend);

  //初始化publisher
  string topicName = "/turtle1/cmd_vel";
  publisher = node->advertise<geometry_msgs::Twist>(topicName, 1000);

  //初始化subscriber
  string poseTopicName = "/turtle1/pose";
  subscriber = node->subscribe(poseTopicName, 1000, &MainWindow2::poseCallback, this);
}

void MainWindow2::clickSend() {
  double linearX = editLinear->text().toDouble();
  double angularZ = editAngular->text().toDouble();

  cout << "linear.x: " << linearX << endl;
  cout << "angular.z: " << angularZ << endl;

  //创建消息
  geometry_msgs::Twist twist;
  //填充消息
  twist.linear.x = linearX;
  twist.angular.z = angularZ * M_PI / 180;
  //发送消息
  publisher.publish(twist);
}

void MainWindow2::poseCallback(const turtlesim::Pose::ConstPtr &message) {
  double x = message->x;
  double y = message->y;
  float linear = message->linear_velocity;
  float angular = message->angular_velocity;
  float theta = message->theta;

  labelX->setText(QString::fromStdString(to_string(x)));
  labelY->setText(QString::fromStdString(to_string(y)));
  labelLinear->setText(QString::fromStdString(to_string(linear)));
  labelAngular->setText(QString::fromStdString(to_string(angular)));
  labelDegrees->setText(QString::fromStdString(to_string(theta * 180 / M_PI)));
}

void MainWindow2::onUpdate() {
  ros::spinOnce();
  update();

  if (!ros::ok()) {
    close();
  }
}

turtle_ctrl2.cpp

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
#include <iostream>
#include "ros/ros.h"
#include <QApplication>
#include "MainWindow2.h"

using namespace std;

int main(int argc, char **argv) {
  string nodeName = "turtle_ctrl2";
  //创建节点
  ros::init(argc, argv, nodeName);
  ros::NodeHandle node;

//  //开启一个异步的轮询器
//  ros::AsyncSpinner spinner(1);
//  spinner.start();

  //Qt程序添加
  QApplication app(argc, argv);

  //创建窗体
  MainWindow2 window(&node);
  window.show();

  return app.exec();
}

MainWindow2.py

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#!/usr/bin/env python
# coding: utf-8

from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from PyQt5.QtGui import *
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import radians, degrees


class MainWindow2(QWidget):
    def __init__(self):
        super(MainWindow2, self).__init__()

        # 自定义刷新
        updateTimer = QTimer(self)
        updateTimer.setInterval(16)
        updateTimer.start()

        updateTimer.timeout.connect(self.onUpdate)

        # 设置title
        self.setWindowTitle("小乌龟控制")
        self.resize(400, 120)

        # 设置布局
        layout = QFormLayout()
        self.setLayout(layout)

        # 添加控件
        self.editLinear = QLineEdit("0")
        layout.addRow("线速度", self.editLinear)

        self.editAngular = QLineEdit("0")
        layout.addRow("角速度", self.editAngular)

        self.labelX = QLabel()
        layout.addRow("当前X坐标", self.labelX)

        self.labelY = QLabel()
        layout.addRow("当前Y坐标", self.labelY)

        self.labelLinear = QLabel()
        layout.addRow("当前线速度", self.labelLinear)

        self.labelAngular = QLabel()
        layout.addRow("当前角速度", self.labelAngular)

        self.labelDegrees = QLabel()
        layout.addRow("当前角度", self.labelDegrees)

        self.btnSend = QPushButton("发送")
        layout.addRow(self.btnSend)

        # 添加事件
        self.btnSend.clicked.connect(self.clickSend)

        # 创建publisher
        topicName = "/turtle1/cmd_vel"
        self.publisher = rospy.Publisher(topicName, Twist, queue_size=1000)

        poseTopicName = "/turtle1/pose"
        rospy.Subscriber(poseTopicName, Pose, self.poseCallback)

    def clickSend(self):
        linearX = float(self.editLinear.text())
        angularZ = radians(float(self.editAngular.text()))

        # 构建消息
        twist = Twist()
        twist.linear.x = linearX
        twist.angular.z = angularZ
        # 发布
        self.publisher.publish(twist)

    def poseCallback(self, msg):
        if not isinstance(msg, Pose):
            return
        self.labelX.setText(str(msg.x))
        self.labelY.setText(str(msg.y))
        self.labelLinear.setText(str(msg.linear_velocity))
        self.labelAngular.setText(str(msg.angular_velocity))
        self.labelDegrees.setText(str(degrees(msg.theta)))

    def onUpdate(self):
        self.update()

        if rospy.is_shutdown():
            self.close()

turtle_ctrl2.py

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
#!/usr/bin/env python
# coding:utf-8

import sys
import rospy
from MainWindow2 import MainWindow2
from PyQt5.QtWidgets import *

if __name__ == '__main__':
    nodeName = "turtle_ctrl2"
    rospy.init_node(nodeName)

    app = QApplication(sys.argv)
    window = MainWindow2()
    window.show()

    sys.exit(app.exec_())