小乌龟控制升级实现

Subscriber整合

1. 添加subscriber创建

self.subscriber = self.node.create_subscription(Pose, '/turtle1/pose', self.pose_call_back,10)

2. 添加订阅回调

def pose_call_back(self, pose):
        self.x_label.setText(str(pose.x))
        self.y_label.setText(str(pose.y))
        self.angle_label.setText(str(pose.theta))
        self.linear_label.setText(str(pose.linear_velocity))
        self.angular_label.setText(str(pose.angular_velocity))

3. 调试

为了能收到订阅的消息,需要定时的调用spin_once获取订阅收topic返回的消息:

1. 定义定时器

updateTimer = QTimer(self)
updateTimer.setInterval(16)
updateTimer.start()

updateTimer.timeout.connect(self.onUpdate)

1. 定时到之后,执行更新操作

def onUpdate(self):
        self.update()
        rclpy.spin_once(self.node)
        if not rclpy.ok():
            self.close()

注意:

需要调用update更新界面

完整示例代码

import rclpy
import sys
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from PyQt5.QtWidgets import QWidget, QApplication, QFormLayout, QLineEdit, QPushButton, QLabel
from PyQt5.QtCore import QTimer


class MainWindow(QWidget):
    def __init__(self, node):
        super(MainWindow, self).__init__()
        self.node = node
        # timer
        updateTimer = QTimer(self)
        updateTimer.setInterval(16)
        updateTimer.start()

        updateTimer.timeout.connect(self.onUpdate)
        # topic
        self.publisher = self.node.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.subscriber = self.node.create_subscription(Pose, '/turtle1/pose', self.pose_call_back,10)

        self.resize(400, 120)
        self.setWindowTitle('小乌龟控制')
        # 布局
        layout = QFormLayout()
        self.setLayout(layout)

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

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

        # x
        self.x_label = QLabel('0')
        # y
        self.y_label = QLabel('0')
        # linear
        self.linear_label = QLabel('0')
        # angular
        self.angular_label = QLabel('0')
        # angle
        self.angle_label = QLabel('0')
        layout.addRow('x坐标', self.x_label)
        layout.addRow('y坐标', self.y_label)
        layout.addRow('线速度', self.linear_label)
        layout.addRow('角速度', self.angular_label)
        layout.addRow('角度', self.angle_label)

        btnSend = QPushButton("发送")
        layout.addRow('', btnSend)
        btnSend.clicked.connect(self.start_control)

    def start_control(self):
        data = Twist()
        data.linear.x = float(self.editLinear.text())
        data.angular.z = float(self.editAngular.text())
        self.publisher.publish(data)

    def pose_call_back(self, pose):
        self.x_label.setText(str(pose.x))
        self.y_label.setText(str(pose.y))
        self.angle_label.setText(str(pose.theta))
        self.linear_label.setText(str(pose.linear_velocity))
        self.angular_label.setText(str(pose.angular_velocity))

    def onUpdate(self):
        self.update()
        rclpy.spin_once(self.node)
        if not rclpy.ok():
            self.close()



def main():
    rclpy.init()
    node = Node('turtlesim_control')
    app = QApplication(sys.argv)
    window = MainWindow(node)
    window.show()
    # rclpy.spin(node)
    result = app.exec_()
    node.destroy_node()
    rclpy.shutdown()
    sys.exit()


if __name__ == '__main__':
    main()