完整代码

  1. import typing
  2. from PyQt5 import QtCore
  3. import rclpy
  4. from rclpy.node import Node
  5. from example_interfaces.srv import AddTwoInts
  6. from PyQt5.QtWidgets import QWidget,QApplication,QFormLayout,QLineEdit,QLabel,QPushButton
  7. from PyQt5.QtCore import QTimer
  8. import sys
  9. from turtlesim.msg import Pose
  10. from geometry_msgs.msg import Twist
  11. from std_msgs.msg import Float64
  12. from threading import Thread
  13. import math
  14. import time
  16. class MainWindow(QWidget):
  17.     def __init__(self, node) -> None:
  18.         super().__init__()
  19.         # 节点
  20.         self.node = node
  21.         # 初始化ui界面
  22.         self.init_ui()
  23.         # 定时器
  24.         timer = QTimer(self)
  25.         timer.setInterval(20)
  26.         timer.start()
  27.         timer.timeout.connect(self.onUpdate)
  29.         # 当前的x y theta
  30.         self.curX = 0
  31.         self.curY = 0
  32.         self.curTheta = 0
  34.         # 订阅位置
  35.         self.sub = self.node.create_subscription(Pose,'/turtle1/pose',self.pose_callback,10)
  36.         # 小乌龟控制发布者
  37.         self.control_pub = self.node.create_publisher(Twist,'/turtle1/cmd_vel',10)
  38.         # 发布小乌龟速度
  39.         self.speed_pub = self.node.create_publisher(Float64,'/turtle/speed',10)
  42.     def pose_callback(self,pose):
  43.         self.curX = pose.x
  44.         self.curY = pose.y
  45.         self.curTheta = pose.theta
  46.         # 设置当前位置
  47.         self.xL.setText(str(self.curX))
  48.         self.yL.setText(str(self.curY))
  49.         self.thetaL.setText(str(self.curTheta))
  51.     # 定时更新
  52.     def onUpdate(self):
  53.         if not rclpy.ok():
  54.             self.close()
  55.             return
  56.         rclpy.spin_once(self.node)
  58.     def init_ui(self):
  59.         # 布局
  60.         layout = QFormLayout()
  61.         self.setLayout(layout)
  63.         self.targetEditX = QLineEdit()
  64.         self.targetEditY = QLineEdit()
  66.         self.xL = QLabel()
  67.         self.yL = QLabel()
  68.         self.thetaL = QLabel()
  70.         self.kpE = QLineEdit()
  71.         self.kiE = QLineEdit()
  72.         self.kdE = QLineEdit()
  74.         executeBtn = QPushButton('执行')
  75.         vSweepBtn = QPushButton('纵向扫地')
  77.         layout.addRow('目标X:',self.targetEditX)
  78.         layout.addRow('目标Y:',self.targetEditY)
  80.         layout.addRow('当前X:',self.xL)
  81.         layout.addRow('当前Y:',self.yL)
  82.         layout.addRow('当前theta:',self.thetaL)
  84.         layout.addRow('KP:',self.kpE)
  85.         layout.addRow('KI:',self.kiE)
  86.         layout.addRow('KD:',self.kdE)
  88.         self.kpE.setText('0.1')
  89.         self.kiE.setText('0.1')
  90.         self.kdE.setText('0.1')
  92.         self.targetEditX.setText('8.544444561')
  93.         self.targetEditY.setText('5.544444561')
  95.         layout.addRow('',executeBtn)
  96.         layout.addRow('',vSweepBtn)
  98.         executeBtn.clicked.connect(self.move)
  99.         vSweepBtn.clicked.connect(self.vSweep)
  101.     def vSweep(self):
  102.         print('zhixing1')
  103.         Thread(target=self.vSweepImpl).start()
  105.     def vSweepImpl(self):
  106.         print('zhixing2')
  107.         for i in range(1,11):
  108.             self.executeMove(i,1)
  109.             self.executeMove(i,10)
  111.     def distance(self,curX,curY,targetX,targetY):
  112.         return math.sqrt(pow(targetX-curX,2)+pow(targetY-curY,2))
  114.     def caclAngle(self,curX,curY,targetX,targetY,theta):
  115.         angle = math.atan2(targetY-curY,targetX-curX)-theta
  116.         if angle>math.pi:
  117.             angle -= 2*math.pi
  118.         elif angle<-math.pi:
  119.             angle += 2*math.pi
  121.         return angle
  124.     def executeMove(self,targetX,targetY):
  125.         dst = self.distance(self.curX,self.curY,targetX,targetY)
  126.         # 定义时间
  127.         control_time = 5.0
  128.         # 掉头时间
  129.         turnTime = control_time/12
  130.         # 睡眠
  131.         hz = 10
  132.         # 频率
  133.         rate = self.node.create_rate(hz)
  134.         # 当前速度
  135.         curSpeed = 0
  136.         # 上一次的速度差
  137.         lastSpeedDiff = 0
  138.         # 累计误差
  139.         totalSpeedDiff = 0
  141.         # kp ki kd
  142.         kp = float(self.kpE.text())
  143.         ki = float(self.kiE.text())
  144.         kd = float(self.kdE.text())
  146.         # 求速度
  147.         while self.distance(self.curX,self.curY,targetX,targetY)>0.1:
  148.             # 目标速度
  149.             speed = self.distance(self.curX,self.curY,targetX,targetY)/control_time
  150.             # kp
  151.             curSpeedDiff = speed-curSpeed
  152.             curSpeed += curSpeedDiff*kp
  153.             # kd
  154.             curSpeed += (curSpeedDiff-lastSpeedDiff)*kd
  155.             lastSpeedDiff = curSpeedDiff
  156.             # ki
  157.             totalSpeedDiff += curSpeedDiff
  158.             curSpeed += totalSpeedDiff*ki
  160.             # 发布控制小乌龟
  161.             data = Twist()
  162.             data.linear.x = curSpeed
  163.             data.angular.z = self.caclAngle(self.curX,self.curY,targetX,targetY,self.curTheta)/turnTime
  164.             self.control_pub.publish(data)
  166.             # 发布速度plot显示
  167.             speed_data = Float64()
  168.             speed_data.data = curSpeed
  169.             self.speed_pub.publish(speed_data)
  171.             rate.sleep()
  172.             control_time -= 1.0/hz
  173.             # time.sleep(0.09)
  175.         print('stop')
  176.         # 发布控制小乌龟
  177.         data = Twist()
  178.         data.linear.x = 0.0
  179.         self.control_pub.publish(data)
  182.     def move(self):
  183.         targetX = float(self.targetEditX.text())
  184.         targetY = float(self.targetEditY.text())
  185.         # 开启线程执行
  186.         Thread(target=self.executeMove,args=(targetX,targetY)).start()
  189. def main():
  190.     rclpy.init()
  191.     node = Node('service_server_node')
  193.     app = QApplication(sys.argv)
  194.     window = MainWindow(node)
  195.     window.show()
  196.     print('app start')
  197.     result = app.exec_()
  198.     node.destroy_node()
  199.     rclpy.shutdown()
  200.     sys.exit(result)
  203. if __name__ == '__main__':
  204.     main()