面向对象Qt源码

MainWindow1.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
#include <QtWidgets>
#include "ros/ros.h"

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

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

  //布局
  QFormLayout *layout;
  //线速度输入框
  QLineEdit *editLinear;
  //角速度输入框
  QLineEdit *editAngular;
  // 发送按钮
  QPushButton *btnSend;

 private:
  //初始化控件和布局
  void initWidgets();
  //发送点击事件
  void clickSend();
};

MainWindow1.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
#include "MainWindow1.h"
#include <iostream>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

using namespace std;

MainWindow1::MainWindow1(ros::NodeHandle *node, QWidget *parent) : QWidget(parent) {
  this->node = node;
  //初始化控件和布局
  initWidgets();
}

MainWindow1::~MainWindow1() = default;

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

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

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

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

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

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

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

void MainWindow1::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);
}

turtle_ctrl1.cpp

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

using namespace std;

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

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

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

  return app.exec();
}

MainWindow1.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
#!/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 math import radians


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

        # 设置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.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)

    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)

turtle_ctrl1.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 MainWindow1 import MainWindow1
from PyQt5.QtWidgets import *

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

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

    sys.exit(app.exec_())