修改代码

src/.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (ven)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (src)" project-jdk-type="Python SDK" />
 </project>

src/.idea/src.iml

@@ -1,8 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.11 (ven)" jdkType="Python SDK" />
+    <content url="file://$MODULE_DIR$">
+      <excludeFolder url="file://$MODULE_DIR$/windows/res/ven" />
+    </content>
+    <orderEntry type="jdk" jdkName="Python 3.11 (src)" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
   <component name="PyNamespacePackagesService">

src/windows/control/controltools/contest(1).py

@@ -0,0 +1,962 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+import tf_transformations
+import threading
+import numpy as np
+import os
+from configparser import ConfigParser
+import pygame
+
+
+class Drive(object):
+    def __init__(self) -> None:
+        self.valueinit()
+        self.ros_register()
+
+    def valueinit(self):
+
+        if os.path.exists('config.ini'):
+            config = ConfigParser()
+            with open('config.ini') as f:
+                config.read_file(f)
+
+                self.threshold_l = float(config.get('threshold', 'threshold_l'))
+                self.threshold_lf = float(config.get('threshold', 'threshold_lf'))
+                self.threshold_f = float(config.get('threshold', 'threshold_f'))
+                self.threshold_rf = float(config.get('threshold', 'threshold_rf'))
+                self.threshold_r = float(config.get('threshold', 'threshold_r'))
+
+                self.fyaw = float(config.get('threshold', 'fyaw'))
+                self.ryaw = float(config.get('threshold', 'ryaw'))
+                self.byaw = float(config.get('threshold', 'byaw'))
+                self.lyaw0 = float(config.get('threshold', 'lyaw0'))
+                self.lyaw1 = float(config.get('threshold', 'lyaw1'))
+
+                self.blocksize = float(config.get('threshold', 'blocksize'))
+                self.kp = float(config.get('threshold', 'kp'))
+
+                self.speed_x = float(config.get('threshold', 'speed_x'))
+                self.speed_z = float(config.get('threshold', 'speed_z'))
+
+        else:
+            self.threshold_l = 0.085
+            self.threshold_lf = 0.12
+            self.threshold_f = 0.078
+            self.threshold_rf = 0.12
+            self.threshold_r = 0.085
+
+            self.fyaw = 4.71
+            self.ryaw = 3.14
+            self.byaw = 1.57
+            self.lyaw0 = 0
+            self.lyaw1 = 6.28
+
+            self.blocksize = 0.17
+            self.kp = 10
+
+            self.speed_x = 0.10
+            self.speed_z = 0.35
+
+        self.yaw = 0
+        self.l_dis = 0
+        self.fl_dis = 0
+        self.f_dis = 0
+        self.fr_dis = 0
+        self.r_dis = 0
+        self.position_x = 0
+        self.position_y = 0
+
+    def ros_register(self):
+        self.msg = Twist()
+        self.string = String()
+        self.node = Node('mynode')
+        self.node.create_subscription(LaserScan, '/scan', self.laser, 10)
+        self.node.create_subscription(Odometry, '/odom', self.odom, 10)
+        self.pub = self.node.create_publisher(Twist, '/cmd_vel', 10)
+        self.pub_custom = self.node.create_publisher(String, '/mycustom', 10)
+        # self.timer = self.node.create_timer(0.05, self.ros_pub)
+        self.rate = self.node.create_rate(50.0)
+
+    def ros_pub(self):
+        self.msg.linear.x = self.speed_x
+        self.msg.angular.z = self.speed_z
+        self.pub.publish(self.msg)
+
+    def ros_spin(self):
+        rclpy.spin(self.node)
+
+    def laser(self, msg):
+        region = msg.ranges
+        self.l_dis = region[340]
+        self.fl_dis = region[270]
+        self.f_dis = region[180]
+        self.fr_dis = region[90]
+        self.r_dis = region[20]
+
+    def get_laser(self):
+        return self.l_dis, self.fl_dis, self.f_dis, self.fr_dis, self.r_dis
+
+    def odom(self, msg):
+        position = msg.pose.pose.position
+        self.position_x = position.x
+        self.position_y = position.y
+        quaternion = (
+            msg.pose.pose.orientation.x,
+            msg.pose.pose.orientation.y,
+            msg.pose.pose.orientation.z,
+            msg.pose.pose.orientation.w)
+        euler = tf_transformations.euler_from_quaternion(quaternion)
+        self.yaw = euler[2] + 3.1415926
+
+    def get_odom(self):
+        return self.position_x, self.position_y, self.yaw
+
+    def posture_adjust(self):
+        if self.f_dis > 0.09:
+            if self.fl_dis < 0.118:
+                speed_z_temp = 0.5 * (self.fl_dis - 0.118) * self.kp
+            elif self.fr_dis < 0.118:
+                speed_z_temp = -0.5 * (self.fr_dis - 0.118) * self.kp
+            else:
+                speed_z_temp = 0.0
+        else:
+            if self.l_dis < 0.085:
+                speed_z_temp = 0.5 * (self.l_dis - 0.085) * self.kp
+            elif self.r_dis < 0.085:
+                speed_z_temp = -0.5 * (self.r_dis - 0.085) * self.kp
+            else:
+                speed_z_temp = 0.0
+        return speed_z_temp
+
+    def move(self, numblock=1):
+        flag = 1
+        flag_cps = 0
+        compensate = 0
+        while rclpy.ok():
+            self.msg.linear.x = self.speed_x
+            self.msg.angular.z = self.posture_adjust()
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+
+            if flag:
+                tempx, tempy, tempz = self.get_odom()
+                if (not tempx) and (not tempy):
+                    continue
+                flag = 0
+                if self.l_dis < 0.1 and self.r_dis < 0.1:
+                    flag_cps = 0
+                elif self.l_dis < 0.1 and self.r_dis > 0.1:
+                    flag_cps = 1
+                if self.l_dis > 0.1 and self.r_dis < 0.1:
+                    flag_cps = 2
+                elif self.l_dis > 0.1 and self.r_dis > 0.1:
+                    flag_cps = 3
+
+            if 0.01 < self.f_dis < 0.075:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+            if abs(self.get_odom()[0] - tempx) >= self.blocksize * numblock or abs(
+                    self.get_odom()[1] - tempy) >= self.blocksize * numblock:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+            if flag_cps == 0 and (self.l_dis > 0.1 or self.r_dis > 0.1):
+                flag = 1
+                compensate = 1
+                break
+
+        """
+            # 其他情况，暂时没想好怎么处理，所以注释掉
+            if flag_cps ==1 and self.l_dis>0.1:
+                flag = 1
+                compensate = 0
+                break
+
+            if flag_cps ==2 and self.r_dis>0.1:
+                flag = 1
+                compensate = 0
+                break
+        """
+
+        while compensate and rclpy.ok():
+            self.msg.linear.x = self.speed_x
+            self.msg.angular.z = self.posture_adjust()
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:
+                tempx, tempy, tempz = self.get_odom()
+                if (not tempx) and (not tempy):
+                    continue
+                flag = 0
+
+            if 0.01 < self.f_dis < 0.075:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+            if abs(self.get_odom()[0] - tempx) >= self.blocksize / 2 + 0.01 or abs(
+                    self.get_odom()[1] - tempy) >= self.blocksize / 2 + 0.01:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+    def turn(self, angle):
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = -self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if angle - 0.1 < self.yaw < angle + 0.1:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+    def turnright(self):
+        flag = 1
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = -self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:  # It is similar with move.
+                oldyaw = self.get_odom()[2]
+                if not oldyaw:
+                    continue
+                flag = 0
+            # Considering the mutation of 0 and 6.28, so dividing the whole process into four parts.
+
+            if 4.41 < oldyaw < 5.01:  # up
+                if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+            elif 2.84 < oldyaw < 3.44:  # right
+                if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+            elif 1.27 < oldyaw < 1.87:  # back
+                if self.yaw < self.lyaw0 + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+            elif oldyaw < 0.3 or oldyaw > 5.98:  # left
+                if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+    def turnleft(self):
+        flag = 1
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:
+                oldyaw = self.yaw
+                if not oldyaw:
+                    continue
+                flag = 0
+            if 4.41 < oldyaw < 5.01:  # up
+                if self.yaw > self.lyaw1 - 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(1111)
+                    break
+            elif 2.84 < oldyaw < 3.44:  # right
+                if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(222)
+                    break
+            elif 1.27 < oldyaw < 1.87:  # back
+                if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(44444)
+                    break
+            elif oldyaw < 0.3 or oldyaw > 5.98:  # left
+                if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(155555)
+                    break
+
+    def turnback(self):
+        flag = 1
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = -self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:
+                oldyaw = self.get_odom()[2]
+                if not oldyaw:
+                    continue
+                flag = 0
+            if 4.41 < oldyaw < 5.01:  # up
+                if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+            elif 2.84 < oldyaw < 3.44:  # right
+                if self.yaw < self.lyaw0 + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+            elif 1.27 < oldyaw < 1.87:  # back
+                if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+            elif oldyaw < 0.3 or oldyaw > 5.98:  # left
+                if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+
+class Micromouse(Drive):
+    def __init__(self):
+        """继承父类，禁止删减，但可以增加自己要添加的内容。"""
+        super().__init__()
+        self.valuesinit()
+
+    def valuesinit(self):
+        """Micromosue参数初始化, 禁止修改"""
+        self.mapblock = np.full((16, 16), '00000')  # 2D array,墙壁资料初始化, flag left down right up
+        # flag: 是否走过, 0 or 1
+        # left: 是否有路, 0 or 1
+        # down: 是否有路, 0 or 1
+        # right: 是否有路, 0 or 1
+        # up: 是否有路, 0 or 1
+
+        self.mapblock[0, 0] = '10001'  # 起点墙壁初始化
+        self.crosswaystack = []
+        self.mouse_x = 0  # x,y起点坐标，这是在迷宫中的坐标
+        self.mouse_y = 0
+        self.dir = 0  # Micromouse车头,0:up, 1:right, 2:down, 3:left
+
+    def __coordinateupdate(self):
+        """根据车头方向更新坐标, 禁止修改"""
+        if self.dir == 0:  # 车头向上
+            self.mouse_y += 1
+        elif self.dir == 1:  # 车头向右
+            self.mouse_x += 1
+        elif self.dir == 2:  # 车头向下
+            self.mouse_y -= 1
+        elif self.dir == 3:  # 车头向左
+            self.mouse_x -= 1
+
+    def get_coordinate(self):
+        """获取坐标，方便调用，禁止修改"""
+        return self.mouse_x, self.mouse_y
+
+    def __savewallinfo(self):
+        """墙壁信息保存, 禁止修改"""
+        frontpath = '1' if self.f_dis > 0.2 else '0'
+        rightpath = '1' if self.r_dis > 0.2 else '0'
+        backpath = '1'
+        leftpath = '1' if self.l_dis > 0.2 else '0'
+        # 墙壁资料赋值
+        if self.dir == 0:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', leftpath, backpath, rightpath, frontpath))
+        elif self.dir == 1:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', backpath, rightpath, frontpath, leftpath))
+        elif self.dir == 2:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', rightpath, frontpath, leftpath, backpath))
+        elif self.dir == 3:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', frontpath, leftpath, backpath, rightpath))
+
+    def turnright(self):
+        """右转弯，加入了方向转换, 禁止修改"""
+        self.dir = (self.dir + 1) % 4
+        return super(Micromouse, self).turnright()
+
+    def turnleft(self):
+        """左转弯，加入了方向转换, 禁止修改"""
+        self.dir = (self.dir + 3) % 4
+        return super(Micromouse, self).turnleft()
+
+    def turnback(self):
+        """向后转弯，加入了方向转换, 禁止修改"""
+        self.dir = (self.dir + 2) % 4
+        return super(Micromouse, self).turnback()
+
+    def moveoneblock(self):
+        """移动一个单元格，然后更新坐标，保存墙壁信息的功能, 禁止修改"""
+        self.move()
+        self.__coordinateupdate()
+        if self.mapblock[self.mouse_x, self.mouse_y][0] == '0':
+            self.__savewallinfo()
+        self.ros_pub_custom()
+
+    def ros_pub_custom(self):
+        data = ''.join((str(self.mouse_x), ',',
+                        str(self.mouse_y), ',',
+                        str(self.dir), ',',
+                        self.mapblock[self.mouse_x, self.mouse_y][1:]
+                        ))
+        self.string.data = data
+        self.pub_custom.publish(self.string)
+
+    def rightmethod(self):
+        frontpath = '1' if self.f_dis > 0.2 else '0'
+        rightpath = '1' if self.r_dis > 0.2 else '0'
+        backpath = '1'
+        leftpath = '1' if self.l_dis > 0.2 else '0'
+        if rightpath == '1':  # 右侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            if flag == '0':
+                self.turnright()
+                return
+
+        if frontpath == '1':  # 前方有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            if flag == '0':
+                return
+
+        if leftpath == '1':  # 左侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            if flag == '0':
+                self.turnleft()
+                return
+
+    def leftmethod(self):
+        frontpath = '1' if self.f_dis > 0.2 else '0'
+        rightpath = '1' if self.r_dis > 0.2 else '0'
+        backpath = '1'
+        leftpath = '1' if self.l_dis > 0.2 else '0'
+        if leftpath == '1':  # 左侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            if flag == '0':
+                self.turnleft()
+                return
+
+        if frontpath == '1':  # 前方有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            if flag == '0':
+                return
+
+        if rightpath == '1':  # 右侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            if flag == '0':
+                self.turnright()
+                return
+
+    def frontrightmethod(self):
+        frontpath = '1' if self.f_dis > 0.2 else '0'
+        rightpath = '1' if self.r_dis > 0.2 else '0'
+        backpath = '1'
+        leftpath = '1' if self.l_dis > 0.2 else '0'
+        if frontpath == '1':  # 前方有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            if flag == '0':
+                return
+
+        if rightpath == '1':  # 右侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            if flag == '0':
+                self.turnright()
+                return
+
+        if leftpath == '1':  # 左侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            if flag == '0':
+                self.turnleft()
+                return
+
+    def frontleftmethod(self):
+        frontpath = '1' if self.f_dis > 0.2 else '0'
+        rightpath = '1' if self.r_dis > 0.2 else '0'
+        backpath = '1'
+        leftpath = '1' if self.l_dis > 0.2 else '0'
+        if frontpath == '1':  # 前方有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            if flag == '0':
+                return
+
+        if leftpath == '1':  # 左侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            if flag == '0':
+                self.turnleft()
+                return
+
+        if rightpath == '1':  # 右侧有路
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            if flag == '0':
+                self.turnright()
+                return
+
+    def centralmethod(self):
+        if self.mouse_x < 8:
+            if self.mouse_y < 8:  # Micromouse位于左下角
+                if self.dir == 0:
+                    self.frontrightmethod()
+                elif self.dir == 1:
+                    self.frontleftmethod()
+                elif self.dir == 2:
+                    self.leftmethod()
+                elif self.dir == 3:
+                    self.rightmethod()
+            else:  # Micromouse位于左上角
+                if self.dir == 0:
+                    self.rightmethod()
+                elif self.dir == 1:
+                    self.frontrightmethod()
+                elif self.dir == 2:
+                    self.frontleftmethod()
+                elif self.dir == 3:
+                    self.leftmethod()
+        else:
+            if self.mouse_y < 8:  # Micromouse位于右下角
+                if self.dir == 0:
+                    self.frontleftmethod()
+                elif self.dir == 1:
+                    self.leftmethod()
+                elif self.dir == 2:
+                    self.rightmethod()
+                elif self.dir == 3:
+                    self.frontrightmethod()
+            else:  # Micromouse位于右下角
+                if self.dir == 0:
+                    self.leftmethod()
+                elif self.dir == 1:
+                    self.rightmethod()
+                elif self.dir == 2:
+                    self.frontrightmethod()
+                elif self.dir == 3:
+                    self.frontleftmethod()
+
+    def centralmethod2(self):
+        if self.mouse_x in (7, 8) or self.mouse_y in (7, 8):
+            if self.mouse_y in (7, 8):
+                if self.mouse_x < 8:
+                    if self.dir == 0:
+                        self.rightmethod()
+                    elif self.dir == 1:
+                        self.frontrightmethod()
+                    elif self.dir == 2:
+                        self.leftmethod()
+                    elif self.dir == 3:
+                        self.frontrightmethod()
+                else:
+                    if self.dir == 0:
+                        self.leftmethod()
+                    elif self.dir == 1:
+                        self.frontrightmethod()
+                    elif self.dir == 2:
+                        self.rightmethod()
+                    elif self.dir == 3:
+                        self.frontrightmethod()
+            if self.mouse_x in (7, 8):
+                if self.mouse_y < 8:
+                    if self.dir == 0:
+                        self.frontrightmethod()
+                    elif self.dir == 1:
+                        self.leftmethod()
+                    elif self.dir == 2:
+                        self.frontrightmethod()
+                    elif self.dir == 3:
+                        self.rightmethod()
+                else:
+                    if self.dir == 0:
+                        self.frontrightmethod()
+                    elif self.dir == 1:
+                        self.rightmethod()
+                    elif self.dir == 2:
+                        self.frontrightmethod()
+                    elif self.dir == 3:
+                        self.leftmethod()
+        elif self.mouse_x < 8:
+            if self.mouse_y < 8:  # Micromouse位于左下角
+                if self.dir == 0:
+                    self.frontrightmethod()
+                elif self.dir == 1:
+                    self.frontleftmethod()
+                elif self.dir == 2:
+                    self.leftmethod()
+                elif self.dir == 3:
+                    self.rightmethod()
+            else:  # Micromouse位于左上角
+                if self.dir == 0:
+                    self.rightmethod()
+                elif self.dir == 1:
+                    self.frontrightmethod()
+                elif self.dir == 2:
+                    self.frontleftmethod()
+                elif self.dir == 3:
+                    self.leftmethod()
+        else:
+            if self.mouse_y < 8:  # Micromouse位于右下角
+                if self.dir == 0:
+                    self.frontleftmethod()
+                elif self.dir == 1:
+                    self.leftmethod()
+                elif self.dir == 2:
+                    self.rightmethod()
+                elif self.dir == 3:
+                    self.frontrightmethod()
+            else:  # Micromouse位于右下角
+                if self.dir == 0:
+                    self.leftmethod()
+                elif self.dir == 1:
+                    self.rightmethod()
+                elif self.dir == 2:
+                    self.frontrightmethod()
+                elif self.dir == 3:
+                    self.frontleftmethod()
+
+    def destinationcheck(self):
+        if (self.mouse_x == 7 and self.mouse_y == 7) or (self.mouse_x == 7 and self.mouse_y == 8) or (
+                self.mouse_x == 8 and self.mouse_y == 7) or (self.mouse_x == 8 and self.mouse_y == 8):
+            return True
+        else:
+            return False
+
+    def crosswaychoice(self):
+        self.centralmethod()
+
+    def mapstepedit(self, dx, dy):
+        """制作等高图, 禁止修改"""
+        self.mapstep = np.full((16, 16), 255)
+        step = 1
+        n = 1
+        stack = []
+        stack.append((dx, dy))
+        cx = dx
+        cy = dy
+
+        while n:
+            self.mapstep[cx, cy] = step
+            step += 1
+
+            count = 0
+            # 统计当前坐标有几个可前进的方向
+            if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] > step):
+                count += 1
+            if (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] > step):
+                count += 1
+            if (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] > step):
+                count += 1
+            if (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] > step):
+                count += 1
+
+            if count == 0:
+                cx, cy = stack.pop()
+                step = self.mapstep[cx, cy]
+                n -= 1
+            else:
+                if count > 1:
+                    stack.append((cx, cy))
+                    n += 1
+                # 随便挑一个方向，不影响结果，因为会全部走一遍
+                if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] > step):
+                    cy += 1
+                    continue
+                if (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] > step):
+                    cx += 1
+                    continue
+                if (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] > step):
+                    cy -= 1
+                    continue
+                if (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] > step):
+                    cx -= 1
+                    continue
+
+    def objectgoto(self, dx, dy):
+        """向目标点运动, 禁止修改"""
+        self.mapstepedit(dx, dy)
+        temp_dir = 0
+        cx = self.mouse_x
+        cy = self.mouse_y
+        while (cx != dx) or (cy != dy):
+            # 沿着等高值减小的方向运行，直至到达目标点
+            step = self.mapstep[cx, cy]
+            if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] < step):
+                temp_dir = 0
+            elif (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] < step):
+                temp_dir = 1
+            elif (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] < step):
+                temp_dir = 2
+            elif (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] < step):
+                temp_dir = 3
+            d_dir = (temp_dir - self.dir + 4) % 4
+            if d_dir == 1:
+                self.turnright()
+            elif d_dir == 2:
+                self.turnback()
+            elif d_dir == 3:
+                self.turnleft()
+            self.moveoneblock()
+            cx = self.mouse_x
+            cy = self.mouse_y
+
+    def crosswaycheck(self):
+        temp = 0
+        left, down, right, up = self.mapblock[self.mouse_x, self.mouse_y][1:]
+        if up == '1':
+            if self.mapblock[self.mouse_x, self.mouse_y + 1][0] == '0':
+                temp += 1
+        if right == '1':
+            if self.mapblock[self.mouse_x + 1, self.mouse_y][0] == '0':
+                temp += 1
+        if down == '1':
+            if self.mapblock[self.mouse_x, self.mouse_y - 1][0] == '0':
+                temp += 1
+        if left == '1':
+            if self.mapblock[self.mouse_x - 1, self.mouse_y][0] == '0':
+                temp += 1
+        return temp
+
+    def mazesearch(self):
+        while 1:
+            # print(self.mouse_x, self.mouse_y)
+            if self.destinationcheck():  # 如果到了终点，则返回起点
+                destination = (self.mouse_x, self.mouse_y)
+                self.turnback()
+                self.objectgoto(0, 0)
+                return (1, destination)
+            else:  # 否则一直搜索
+                crosswaycount = self.crosswaycheck()
+                if crosswaycount:
+                    if crosswaycount > 1:
+                        self.crosswaystack.append((self.mouse_x, self.mouse_y))
+                        self.crosswaychoice()
+                        self.moveoneblock()
+                    if crosswaycount == 1:
+                        self.crosswaychoice()
+                        self.moveoneblock()
+                else:
+                    self.turnback()
+                    self.objectgoto(*self.crosswaystack.pop())
+
+    def spurt(self, dest):
+        self.objectgoto(*dest)
+        self.turnback()
+        self.objectgoto(0, 0)
+
+
+def main():
+    rclpy.init(args=None)
+    micromouse = Micromouse()
+    t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
+    t.start()
+    flag, destination = micromouse.mazesearch()
+    micromouse.spurt(destination)
+
+
+def main2():
+    """ remote control """
+    flag = -1
+    pygame.init()
+    if pygame.joystick.get_count():
+        stick = pygame.joystick.Joystick(0)
+    else:
+        print('No joystick found!!!')
+    rclpy.init(args=None)
+    micromouse = Micromouse()
+    t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
+    t.start()
+    while 1:
+        events = pygame.event.get()
+        if events:
+            for event in events:
+                if event.type == pygame.JOYBUTTONDOWN:
+                    if event.button == 0:
+                        flag = 0
+                        print('go')
+                    if event.button == 1:
+                        flag = 1
+                        print('turn right')
+                    if event.button == 2:
+                        flag = 2
+                        print('turn back')
+                    if event.button == 3:
+                        flag = 3
+                        print('turn left')
+                    if event.button == 4:
+                        flag = 66
+                        print('go back')
+                    if event.button == 5:
+                        flag = 88
+                        print('spurt')
+
+                if event.type == pygame.JOYBUTTONUP:
+                    if event.button in (0, 1, 2, 3):
+                        print('stop')
+                        flag = -1
+
+        match flag:
+            case 0:
+                micromouse.moveoneblock()
+            case 1:
+                micromouse.turnright()
+            case 2:
+                micromouse.turnback()
+            case 3:
+                micromouse.turnleft()
+
+            case 66:
+                destination = (micromouse.mouse_x, micromouse.mouse_y)
+                micromouse.turnback()
+                micromouse.objectgoto(0, 0)
+                flag = -1
+
+            case 88:
+                print(destination)
+                micromouse.spurt(destination)
+                break
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    finally:
+        rclpy.shutdown()

src/windows/control/controltools/contest.py

@@ -0,0 +1,831 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+import tf_transformations
+import threading
+import numpy as np
+import os
+from configparser import ConfigParser
+import pygame
+
+
+class Drive(object):
+    def __init__(self) -> None:
+        self.valueinit()
+        self.ros_register()
+
+    def valueinit(self):
+
+        if os.path.exists('config.ini'):
+            config = ConfigParser()
+            with open('config.ini') as f:
+                config.read_file(f)
+
+                self.threshold_l = float(config.get('threshold', 'threshold_l'))
+                self.threshold_lf = float(config.get('threshold', 'threshold_lf'))
+                self.threshold_f = float(config.get('threshold', 'threshold_f'))
+                self.threshold_rf = float(config.get('threshold', 'threshold_rf'))
+                self.threshold_r = float(config.get('threshold', 'threshold_r'))
+
+                self.fyaw = float(config.get('threshold', 'fyaw'))
+                self.ryaw = float(config.get('threshold', 'ryaw'))
+                self.byaw = float(config.get('threshold', 'byaw'))
+                self.lyaw0 = float(config.get('threshold', 'lyaw0'))
+                self.lyaw1 = float(config.get('threshold', 'lyaw1'))
+
+                self.blocksize = float(config.get('threshold', 'blocksize'))
+                self.kp = float(config.get('threshold', 'kp'))
+
+                self.speed_x = float(config.get('threshold', 'speed_x'))
+                self.speed_z = float(config.get('threshold', 'speed_z'))
+
+        else:
+            self.threshold_l = 0.085
+            self.threshold_lf = 0.12
+            self.threshold_f = 0.078
+            self.threshold_rf = 0.12
+            self.threshold_r = 0.085
+
+            self.fyaw = 4.71
+            self.ryaw = 3.14
+            self.byaw = 1.57
+            self.lyaw0 = 0
+            self.lyaw1 = 6.28
+
+            self.blocksize = 0.17
+            self.kp = 10
+
+            self.speed_x = 0.10
+            self.speed_z = 0.35
+
+        self.yaw = 0
+        self.l_dis = 0
+        self.fl_dis = 0
+        self.f_dis = 0
+        self.fr_dis = 0
+        self.r_dis = 0
+        self.position_x = 0
+        self.position_y = 0
+
+    def ros_register(self):
+        self.msg = Twist()
+        self.string = String()
+        self.node = Node('mynode')
+        self.node.create_subscription(LaserScan, '/scan', self.laser, 10)
+        self.node.create_subscription(Odometry, '/odom', self.odom, 10)
+        self.pub = self.node.create_publisher(Twist, '/cmd_vel', 10)
+        self.pub_custom = self.node.create_publisher(String, '/mycustom', 10)
+        # self.timer = self.node.create_timer(0.05, self.ros_pub)
+        self.rate = self.node.create_rate(50.0)
+
+    def ros_pub(self):
+        self.msg.linear.x = self.speed_x
+        self.msg.angular.z = self.speed_z
+        self.pub.publish(self.msg)
+
+    def ros_spin(self):
+        rclpy.spin(self.node)
+
+    def laser(self, msg):
+        region = msg.ranges
+        self.l_dis = region[340]
+        self.fl_dis = region[270]
+        self.f_dis = region[180]
+        self.fr_dis = region[90]
+        self.r_dis = region[20]
+
+    def get_laser(self):
+        return self.l_dis, self.fl_dis, self.f_dis, self.fr_dis, self.r_dis
+
+    def odom(self, msg):
+        position = msg.pose.pose.position
+        self.position_x = position.x
+        self.position_y = position.y
+        quaternion = (
+            msg.pose.pose.orientation.x,
+            msg.pose.pose.orientation.y,
+            msg.pose.pose.orientation.z,
+            msg.pose.pose.orientation.w)
+        euler = tf_transformations.euler_from_quaternion(quaternion)
+        self.yaw = euler[2] + 3.1415926
+
+    def get_odom(self):
+        return self.position_x, self.position_y, self.yaw
+
+    def posture_adjust(self):
+        if self.f_dis > 0.09:
+            if self.fl_dis < 0.118:
+                speed_z_temp = 0.5 * (self.fl_dis - 0.118) * self.kp
+            elif self.fr_dis < 0.118:
+                speed_z_temp = -0.5 * (self.fr_dis - 0.118) * self.kp
+            else:
+                speed_z_temp = 0.0
+        else:
+            if self.l_dis < 0.085:
+                speed_z_temp = 0.5 * (self.l_dis - 0.085) * self.kp
+            elif self.r_dis < 0.085:
+                speed_z_temp = -0.5 * (self.r_dis - 0.085) * self.kp
+            else:
+                speed_z_temp = 0.0
+        return speed_z_temp
+
+    def move(self, numblock=1):
+        flag = 1
+        flag_cps = 0
+        compensate = 0
+        while rclpy.ok():
+            self.msg.linear.x = self.speed_x
+            self.msg.angular.z = self.posture_adjust()
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+
+            if flag:
+                tempx, tempy, tempz = self.get_odom()
+                if (not tempx) and (not tempy):
+                    continue
+                flag = 0
+                if self.l_dis < 0.1 and self.r_dis < 0.1:
+                    flag_cps = 0
+                elif self.l_dis < 0.1 and self.r_dis > 0.1:
+                    flag_cps = 1
+                if self.l_dis > 0.1 and self.r_dis < 0.1:
+                    flag_cps = 2
+                elif self.l_dis > 0.1 and self.r_dis > 0.1:
+                    flag_cps = 3
+
+            if 0.01 < self.f_dis < 0.075:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+            if abs(self.get_odom()[0] - tempx) >= self.blocksize * numblock or abs(
+                    self.get_odom()[1] - tempy) >= self.blocksize * numblock:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+            if flag_cps == 0 and (self.l_dis > 0.1 or self.r_dis > 0.1):
+                flag = 1
+                compensate = 1
+                break
+
+        """
+            # 其他情况，暂时没想好怎么处理，所以注释掉
+            if flag_cps ==1 and self.l_dis>0.1:
+                flag = 1
+                compensate = 0
+                break
+
+            if flag_cps ==2 and self.r_dis>0.1:
+                flag = 1
+                compensate = 0
+                break
+        """
+
+        while compensate and rclpy.ok():
+            self.msg.linear.x = self.speed_x
+            self.msg.angular.z = self.posture_adjust()
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:
+                tempx, tempy, tempz = self.get_odom()
+                if (not tempx) and (not tempy):
+                    continue
+                flag = 0
+
+            if 0.01 < self.f_dis < 0.075:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+            if abs(self.get_odom()[0] - tempx) >= self.blocksize / 2 + 0.01 or abs(
+                    self.get_odom()[1] - tempy) >= self.blocksize / 2 + 0.01:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+    def turn(self, angle):
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = -self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if angle - 0.1 < self.yaw < angle + 0.1:
+                self.msg.linear.x = 0.0
+                self.msg.angular.z = 0.0
+                self.pub.publish(self.msg)
+                # rclpy.spin_once(self.node)
+                self.rate.sleep()
+                break
+
+    def turnright(self):
+        flag = 1
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = -self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:  # It is similar with move.
+                oldyaw = self.get_odom()[2]
+                if not oldyaw:
+                    continue
+                flag = 0
+            # Considering the mutation of 0 and 6.28, so dividing the whole process into four parts.
+
+            if 4.41 < oldyaw < 5.01:  # up
+                if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+            elif 2.84 < oldyaw < 3.44:  # right
+                if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+            elif 1.27 < oldyaw < 1.87:  # back
+                if self.yaw < self.lyaw0 + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+            elif oldyaw < 0.3 or oldyaw > 5.98:  # left
+                if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+    def turnleft(self):
+        flag = 1
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:
+                oldyaw = self.yaw
+                if not oldyaw:
+                    continue
+                flag = 0
+            if 4.41 < oldyaw < 5.01:  # up
+                if self.yaw > self.lyaw1 - 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(1111)
+                    break
+            elif 2.84 < oldyaw < 3.44:  # right
+                if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(222)
+                    break
+            elif 1.27 < oldyaw < 1.87:  # back
+                if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(44444)
+                    break
+            elif oldyaw < 0.3 or oldyaw > 5.98:  # left
+                if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    print(155555)
+                    break
+
+    def turnback(self):
+        flag = 1
+        while rclpy.ok():
+            self.msg.linear.x = 0.0
+            self.msg.angular.z = -self.speed_z
+            self.pub.publish(self.msg)
+            self.rate.sleep()
+            if flag:
+                oldyaw = self.get_odom()[2]
+                if not oldyaw:
+                    continue
+                flag = 0
+            if 4.41 < oldyaw < 5.01:  # up
+                if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+            elif 2.84 < oldyaw < 3.44:  # right
+                if self.yaw < self.lyaw0 + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+            elif 1.27 < oldyaw < 1.87:  # back
+                if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+            elif oldyaw < 0.3 or oldyaw > 5.98:  # left
+                if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
+                    self.msg.linear.x = 0.0
+                    self.msg.angular.z = 0.0
+                    self.pub.publish(self.msg)
+                    # rclpy.spin_once(self.node)
+                    self.rate.sleep()
+                    break
+
+
+class Micromouse(Drive):
+    def __init__(self):
+        """Inherits from the parent class, do not remove, but you can add your own content."""
+        super().__init__()
+        self.values_init()
+
+    def values_init(self):
+        """Micromouse parameter initialization, do not modify."""
+        # 2D array to store wall information:
+        # flag: Whether visited, 0 or 1
+        # left: Whether there is a path, 0 or 1
+        # down: Whether there is a path, 0 or 1
+        # right: Whether there is a path, 0 or 1
+        # up: Whether there is a path, 0 or 1
+        self.mapblock = np.full((16, 16), '00000')
+        self.mapblock[0, 0] = '10001'  # Initialize starting wall
+        self.crosswaystack = []
+        self.mouse_x = 0  # Starting coordinates in the maze
+        self.mouse_y = 0
+        self.dir = 0  # Micromouse heading: 0: up, 1: right, 2: down, 3: left
+
+    def coordinate_update(self):
+        """Update coordinates based on the heading direction, do not modify."""
+        if self.dir == 0:  # Heading up
+            self.mouse_y += 1
+        elif self.dir == 1:  # Heading right
+            self.mouse_x += 1
+        elif self.dir == 2:  # Heading down
+            self.mouse_y -= 1
+        elif self.dir == 3:  # Heading left
+            self.mouse_x -= 1
+
+    def get_coordinate(self):
+        """Get coordinates for easy access, do not modify."""
+        return self.mouse_x, self.mouse_y
+
+    def save_wall_info(self):
+        """
+        Save wall information.
+        Do not modify this function.
+        """
+        # Determine if there is a path in each direction based on distance sensors
+        front_path = '1' if self.f_dis > 0.2 else '0'
+        right_path = '1' if self.r_dis > 0.2 else '0'
+        back_path = '1'
+        left_path = '1' if self.l_dis > 0.2 else '0'
+
+        # Assign wall information based on the current direction
+        if self.dir == 0:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', left_path, back_path, right_path, front_path))
+        elif self.dir == 1:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', back_path, right_path, front_path, left_path))
+        elif self.dir == 2:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', right_path, front_path, left_path, back_path))
+        elif self.dir == 3:
+            self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', front_path, left_path, back_path, right_path))
+
+    def turnright(self):
+        """右转弯，加入了方向转换, 禁止修改"""
+        self.dir = (self.dir + 1) % 4
+        return super(Micromouse, self).turnright()
+
+    def turnleft(self):
+        """左转弯，加入了方向转换, 禁止修改"""
+        self.dir = (self.dir + 3) % 4
+        return super(Micromouse, self).turnleft()
+
+    def turnback(self):
+        """向后转弯，加入了方向转换, 禁止修改"""
+        self.dir = (self.dir + 2) % 4
+        return super(Micromouse, self).turnback()
+
+    def move_one_block(self):
+        """
+        Move one block, then update coordinates, save wall information, and publish ROS custom message.
+        Do not modify this function.
+        """
+        self.move()
+        self.update_coordinates()
+        if self.map_block[self.mouse_x, self.mouse_y][0] == '0':
+            self.save_wall_info()
+        self.publish_ros_custom()
+
+    def publish_ros_custom(self):
+        """
+        Publish custom ROS message containing mouse coordinates, direction, and wall information.
+        Do not modify this function.
+        """
+        data = ''.join((str(self.mouse_x), ',',
+                        str(self.mouse_y), ',',
+                        str(self.dir), ',',
+                        self.map_block[self.mouse_x, self.mouse_y][1:]
+                        ))
+        self.string.data = data
+        self.pub_custom.publish(self.string)
+
+    def check_path(self, direction, distance_threshold=0.2):
+        """
+        Check if there's a path in the specified direction with a given distance threshold.
+        Return True if path is clear, otherwise False.
+        """
+        if direction == 'front':
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+        elif direction == 'right':
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x][self.mouse_y - 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x][self.mouse_y + 1][0]
+        elif direction == 'left':
+            if self.dir == 0:
+                flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
+            elif self.dir == 1:
+                flag = self.mapblock[self.mouse_x][self.mouse_y + 1][0]
+            elif self.dir == 2:
+                flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
+            elif self.dir == 3:
+                flag = self.mapblock[self.mouse_x][self.mouse_y - 1][0]
+        return flag == '0' and self.get_distance(direction) > distance_threshold
+
+    def right_method(self):
+        if self.check_path('right'):
+            self.turn_right()
+        elif self.check_path('front'):
+            pass
+        elif self.check_path('left'):
+            self.turn_left()
+
+    def left_method(self):
+        if self.check_path('left'):
+            self.turn_left()
+        elif self.check_path('front'):
+            pass
+        elif self.check_path('right'):
+            self.turn_right()
+
+    def front_right_method(self):
+        if self.check_path('front'):
+            pass
+        elif self.check_path('right'):
+            self.turn_right()
+        elif self.check_path('left'):
+            self.turn_left()
+
+    def front_left_method(self):
+        if self.check_path('front'):
+            pass
+        elif self.check_path('left'):
+            self.turn_left()
+        elif self.check_path('right'):
+            self.turn_right()
+
+    def central_method(self):
+        if self.mouse_x in (7, 8) or self.mouse_y in (7, 8):
+            if self.mouse_y in (7, 8):
+                if self.mouse_x < 8:
+                    if self.dir == 0:
+                        self.right_method()
+                    elif self.dir == 1:
+                        self.front_right_method()
+                    elif self.dir == 2:
+                        self.left_method()
+                    elif self.dir == 3:
+                        self.front_right_method()
+                else:
+                    if self.dir == 0:
+                        self.left_method()
+                    elif self.dir == 1:
+                        self.front_right_method()
+                    elif self.dir == 2:
+                        self.right_method()
+                    elif self.dir == 3:
+                        self.front_right_method()
+            if self.mouse_x in (7, 8):
+                if self.mouse_y < 8:
+                    if self.dir == 0:
+                        self.front_right_method()
+                    elif self.dir == 1:
+                        self.left_method()
+                    elif self.dir == 2:
+                        self.front_right_method()
+                    elif self.dir == 3:
+                        self.right_method()
+                else:
+                    if self.dir == 0:
+                        self.front_right_method()
+                    elif self.dir == 1:
+                        self.right_method()
+                    elif self.dir == 2:
+                        self.front_right_method()
+                    elif self.dir == 3:
+                        self.left_method()
+        else:
+            if self.mouse_x < 8:
+                if self.mouse_y < 8:
+                    if self.dir == 0:
+                        self.front_right_method()
+                    elif self.dir == 1:
+                        self.front_left_method()
+                    elif self.dir == 2:
+                        self.left_method()
+                    elif self.dir == 3:
+                        self.right_method()
+                else:
+                    if self.dir == 0:
+                        self.right_method()
+                    elif self.dir == 1:
+                        self.front_right_method()
+                    elif self.dir == 2:
+                        self.front_left_method()
+                    elif self.dir == 3:
+                        self.left_method()
+            else:
+                if self.mouse_y < 8:
+                    if self.dir == 0:
+                        self.front_left_method()
+                    elif self.dir == 1:
+                        self.left_method()
+                    elif self.dir == 2:
+                        self.right_method()
+                    elif self.dir == 3:
+                        self.front_right_method()
+                else:
+                    if self.dir == 0:
+                        self.left_method()
+                    elif self.dir == 1:
+                        self.right_method()
+                    elif self.dir == 2:
+                        self.front_right_method()
+                    elif self.dir == 3:
+                        self.front_left_method()
+
+    def destination_check(self):
+        """Check if the mouse is at one of the destination points."""
+        destination_points = [(7, 7), (7, 8), (8, 7), (8, 8)]
+        return (self.mouse_x, self.mouse_y) in destination_points
+
+    def crossway_choice(self):
+        """Make a choice at a crossway."""
+        self.central_method()
+
+    def map_step_edit(self, dx, dy):
+        """Create a contour map, not to be modified."""
+        self.map_step = np.full((16, 16), 255)
+        step = 1
+        n = 1
+        stack = []
+        stack.append((dx, dy))
+        cx = dx
+        cy = dy
+
+        while n:
+            self.map_step[cx, cy] = step
+            step += 1
+
+            count = 0
+            # Count available directions at the current coordinate
+            if (self.map_block[cx, cy][-1] == '1') and (self.map_step[cx, cy + 1] > step):
+                count += 1
+            if (self.map_block[cx, cy][-2] == '1') and (self.map_step[cx + 1, cy] > step):
+                count += 1
+            if (self.map_block[cx, cy][-3] == '1') and (self.map_step[cx, cy - 1] > step):
+                count += 1
+            if (self.map_block[cx, cy][-4] == '1') and (self.map_step[cx - 1, cy] > step):
+                count += 1
+
+            if count == 0:
+                cx, cy = stack.pop()
+                step = self.map_step[cx, cy]
+                n -= 1
+            else:
+                if count > 1:
+                    stack.append((cx, cy))
+                    n += 1
+                # Choose a direction, as all will be traversed
+                if (self.map_block[cx, cy][-1] == '1') and (self.map_step[cx, cy + 1] > step):
+                    cy += 1
+                    continue
+                if (self.map_block[cx, cy][-2] == '1') and (self.map_step[cx + 1, cy] > step):
+                    cx += 1
+                    continue
+                if (self.map_block[cx, cy][-3] == '1') and (self.map_step[cx, cy - 1] > step):
+                    cy -= 1
+                    continue
+                if (self.map_block[cx, cy][-4] == '1') and (self.map_step[cx - 1, cy] > step):
+                    cx -= 1
+                    continue
+
+    def objectgoto(self, dx, dy):
+        """向目标点运动, 禁止修改"""
+        self.mapstepedit(dx, dy)
+        temp_dir = 0
+        cx = self.mouse_x
+        cy = self.mouse_y
+        while (cx != dx) or (cy != dy):
+            # 沿着等高值减小的方向运行，直至到达目标点
+            step = self.mapstep[cx, cy]
+
+            # 检查周围是否有墙，如果没有则更新方向
+            if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] < step):
+                temp_dir = 0
+            elif (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] < step):
+                temp_dir = 1
+            elif (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] < step):
+                temp_dir = 2
+            elif (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] < step):
+                temp_dir = 3
+
+            d_dir = (temp_dir - self.dir + 4) % 4
+            if d_dir == 1:
+                self.turnright()
+            elif d_dir == 2:
+                self.turnback()
+            elif d_dir == 3:
+                self.turnleft()
+
+            self.moveoneblock()
+            cx = self.mouse_x
+            cy = self.mouse_y
+
+    def crosswaycheck(self):
+        temp = 0
+        left, down, right, up = self.mapblock[self.mouse_x, self.mouse_y][1:]
+
+        # 检查周围是否有路，如果有则计数
+        if up == '1':
+            if self.mapblock[self.mouse_x, self.mouse_y + 1][0] == '0':
+                temp += 1
+        if right == '1':
+            if self.mapblock[self.mouse_x + 1, self.mouse_y][0] == '0':
+                temp += 1
+        if down == '1':
+            if self.mapblock[self.mouse_x, self.mouse_y - 1][0] == '0':
+                temp += 1
+        if left == '1':
+            if self.mapblock[self.mouse_x - 1, self.mouse_y][0] == '0':
+                temp += 1
+
+        return temp
+
+    def mazesearch(self):
+        while 1:
+            # print(self.mouse_x, self.mouse_y)
+            if self.destinationcheck():
+                # 如果到了终点，则返回起点
+                destination = (self.mouse_x, self.mouse_y)
+                self.turnback()
+                self.objectgoto(0, 0)
+                return (1, destination)
+            else:
+                # 否则一直搜索
+                crosswaycount = self.crosswaycheck()
+                if crosswaycount:
+                    if crosswaycount > 1:
+                        self.crosswaystack.append((self.mouse_x, self.mouse_y))
+                        self.crosswaychoice()
+                        self.moveoneblock()
+                    if crosswaycount == 1:
+                        self.crosswaychoice()
+                        self.moveoneblock()
+                else:
+                    self.turnback()
+                    self.objectgoto(*self.crosswaystack.pop())
+
+    def spurt(self, dest):
+        """快速到达目的地并返回起点"""
+        self.objectgoto(*dest)
+        self.turnback()
+        self.objectgoto(0, 0)
+
+
+def main():
+    """主程序1 - 自动模式"""
+    rclpy.init(args=None)
+    micromouse = Micromouse()
+    t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
+    t.start()
+    flag, destination = micromouse.mazesearch()
+    micromouse.spurt(destination)
+
+
+def main2():
+    """主程序2 - 遥控模式"""
+    flag = -1
+    pygame.init()
+    stick = pygame.joystick.Joystick(0) if pygame.joystick.get_count() else None
+
+    if not stick:
+        print('No joystick found!!!')
+
+    rclpy.init(args=None)
+    micromouse = Micromouse()
+    t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
+    t.start()
+
+    while 1:
+        events = pygame.event.get()
+
+        if events:
+            for event in events:
+                if event.type == pygame.JOYBUTTONDOWN:
+                    button_actions = {
+                        0: ('go', micromouse.moveoneblock),
+                        1: ('turn right', micromouse.turnright),
+                        2: ('turn back', micromouse.turnback),
+                        3: ('turn left', micromouse.turnleft),
+                        4: ('go back', micromouse.goBack),
+                        5: ('spurt', micromouse.spurt)
+                    }
+                    button = event.button
+
+                    if button in button_actions:
+                        flag = button
+                        print(button_actions[button][0])
+                        button_actions[button][1]()
+                    else:
+                        print(f'Button {button} has no action assigned.')
+
+                if event.type == pygame.JOYBUTTONUP:
+                    if event.button in (0, 1, 2, 3):
+                        print('stop')
+                        flag = -1
+
+        match flag:
+            case 66:
+                destination = (micromouse.mouse_x, micromouse.mouse_y)
+                micromouse.turnback()
+                micromouse.objectgoto(0, 0)
+                flag = -1
+
+            case 88:
+                print(destination)
+                micromouse.spurt(destination)
+                break
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    finally:
+        rclpy.shutdown()

src/windows/control/controltools/regPictrue.py

@@ -0,0 +1,61 @@
+import time
+from time import sleep
+
+import numpy as np
+from PyQt5.QtCore import QThread, pyqtSignal
+
+import pyautogui as p
+from paddleocr import PaddleOCR
+
+from windows.res.tools.translate import translate_text
+
+
+class RegPictrue(QThread):
+    org_words = pyqtSignal(str)
+    translate_words = pyqtSignal(str)
+
+    def __init__(self, shotArea, window):
+        super(RegPictrue, self).__init__()
+        self.shotArea = shotArea
+        self.ocr = PaddleOCR(use_angle_cls=True, lang="ch")  # 初始化OCR模型
+        self.translate_window = window
+        self.isOpen = True
+        self.last_words = ''
+
+    def run(self):
+        while self.isOpen:
+            screenshot = p.screenshot(region=(self.shotArea['left'], self.shotArea['top'],
+                                              self.shotArea['width'], self.shotArea['height']))
+            screenshot_np = np.array(screenshot)  # 将Pillow图像对象转换为np.ndarray类型
+            result = self.ocr.ocr(screenshot_np, cls=False)
+            orgwords = ''
+            for idx in result:
+                res = idx[1][0]
+                orgwords += res
+
+            self.org_words.emit(orgwords)
+
+            # 设置源语言和目标语言
+            from_language = 'auto'  # 自动检测语言
+            to_language = 'zh'  # 翻译成中文
+
+            if orgwords != self.last_words:
+
+                # 记录代码开始执行的时间
+                start_time = time.time()
+
+                # 调用翻译函数并输出结果
+                translated_text = translate_text(orgwords, from_language, to_language)
+                if translated_text:
+                    self.translate_words.emit(translated_text)
+                else:
+                    self.translate_words.emit("翻译失败。")
+
+                # 记录代码结束执行的时间
+                end_time = time.time()
+                # 计算代码的运行时间（以秒为单位）
+                execution_time = end_time - start_time
+                print(f"代码执行时间：{execution_time}秒")
+
+            self.last_words = orgwords
+            sleep(0.1)

src/windows/control/controltools/setShotArea.py

@@ -1,21 +1,25 @@
 import pyautogui
 import cv2
 import numpy as np
+from PIL.Image import Image
+from paddleocr import PaddleOCR
+from paddleocr.tools.infer.utility import draw_ocr
 
 
 class SetShotArea():
-    # 获取屏幕宽度和高度
-    screen_width, screen_height = pyautogui.size()
+    def __init__(self):
+        # 获取屏幕宽度和高度
+        self.screen_width, self.screen_height = pyautogui.size()
 
-    # 用于存储用户绘制的矩形坐标
-    rect_coordinates = []
+        # 用于存储用户绘制的矩形坐标
+        self.rect_coordinates = []
 
-    # 获取屏幕截图
-    capture = pyautogui.screenshot()
+        # 获取屏幕截图
+        self.capture = pyautogui.screenshot()
 
-    # 将截图转换为OpenCV格式
-    capture = np.array(capture)
-    capture = cv2.cvtColor(capture, cv2.COLOR_RGB2BGR)
+        # 将截图转换为OpenCV格式
+        self.capture = np.array(self.capture)
+        self.capture = cv2.cvtColor(self.capture, cv2.COLOR_RGB2BGR)
 
     # 回调函数，用于处理鼠标事件
     def on_mouse(self, event, x, y, flags, param):
@@ -54,23 +58,12 @@ class SetShotArea():
                 if len(rect_coordinates) == 2:
                     x1, y1 = rect_coordinates[0]
                     x2, y2 = rect_coordinates[1]
-                    # 截取矩形区域
-                    cropped_area = self.capture[min(y1, y2):max(y1, y2), min(x1, x2):max(x1, x2)]
 
                     shotArea = {'left': min(x1, x2), 'top': min(y1, y2), 'width': max(x1, x2) - min(x1, x2),
                                 'height': max(y1, y2) - min(y1, y2)}
-                    print(shotArea)
+                    cv2.destroyAllWindows()
                     return shotArea
 
-                    # 保存截图
-                    # cv2.imwrite("cropped_area.png", cropped_area)
-                    # print("截图已保存为cropped_area(%d,%d;%d,%d).png" % (min(y1, y2), max(y1, y2), min(x1, x2), max(x1, x2)))
                     break
                 else:
                     print("请先绘制矩形区域再按下'Enter'键进行截图！")
-
-        cv2.destroyAllWindows()
-
-
-it = SetShotArea()
-q = it.setShotArea()

src/windows/control/database/databaseconnact.py

@@ -1,10 +1,9 @@
-import psycopg2
-# -*- coding: utf-8 -*-
+
 import psycopg2
 # 获得连接
-conn = psycopg2.connect(database="gamerule", user="postgres", password="123456", host="localhost", port="5432")
+gamerule = psycopg2.connect(database="gamerule", user="postgres", password="123456", host="localhost", port="5432")
 # 获得游标对象
-cursor = conn.cursor()
+cursor = gamerule.cursor()
 
 sql = "SELECT * FROM table1;"
 # 执行SQL查询
@@ -19,5 +18,5 @@ for row in rows:
 
 # conn.commit()
 # 关闭数据库连接
-conn.close()
+gamerule.close()
 

src/windows/main_window.py

@@ -1,6 +1,5 @@
 import sys
 
-import pyautogui as p
 from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QListWidget, QListWidgetItem, QComboBox
 from PyQt5.QtCore import Qt
 from PyQt5 import uic
@@ -8,7 +7,8 @@ from PyQt5 import uic
 from translate_window import translate
 from settingrule_window import settingrule
 from selectRuleOrder import orderselet
-from control.contrltools.setShotArea import SetShotArea
+from control.controltools.setShotArea import SetShotArea
+from windows.control.controltools.regPictrue import RegPictrue
 
 
 class MyApp(QMainWindow):
@@ -17,7 +17,9 @@ class MyApp(QMainWindow):
 
         uic.loadUi('res/ui/main_window.ui', self)
 
-        self.shotArea = {}
+        self.shotArea = None
+        self.regPicture = None
+        self.translate_state = False
 
         self.translate_window = translate()
         self.setRule_window = settingrule()
@@ -34,7 +36,7 @@ class MyApp(QMainWindow):
 
         self.language.activated.connect(self.selectTransLanguage)
 
-        self.choseLocationBtn.clicked.connect(self.takeScreenShot)
+        self.choseLocationBtn.clicked.connect(self.setShotAreaFun)
 
         self.addRuleBtn.clicked.connect(self.show_settingrule_window)
         self.translateBtn.clicked.connect(self.show_translate_window)
@@ -47,11 +49,9 @@ class MyApp(QMainWindow):
         self.ruleList.itemClicked.connect(self.onItemClicked)
         self.listIndexs = self.get_item_indexs(self.ruleList)
 
-    def takeScreenShot(self):  # 截屏
+    def setShotAreaFun(self):  # 设置截图区域
         shot = SetShotArea()
         self.shotArea = shot.setShotArea()
-        screenshot = p.screenshot(region=(self.shotArea['left'], self.shotArea['top'],
-                                          self.shotArea['width'], self.shotArea['height']))
 
     def show_orderselect_window(self):
         item_states = self.get_item_states(self.ruleList)
@@ -67,7 +67,37 @@ class MyApp(QMainWindow):
         self.setRule_window.show()
 
     def show_translate_window(self):
-        self.translate_window.show()
+        self.change_translate_state()
+
+        if self.translate_state is False:
+            self.translate_window.close()
+            if self.regPicture is not None:
+                self.regPicture.isOpen = False
+
+        elif self.translate_state is True:
+            self.translate_window.show()
+            if self.shotArea is not None:
+                self.regPicture = RegPictrue(self.shotArea, self.translate_window)
+                self.regPicture.org_words.connect(self.orgWords)
+                self.regPicture.translate_words.connect(self.translate_words)
+                self.regPicture.start()
+
+    def orgWords(self, owords):
+        self.translate_window.orgwords.setText(owords)
+
+    def translate_words(self, twords):
+        self.translate_window.transwords.setText(twords)
+
+    def change_translate_state(self):
+        # 处理项目的选择状态变化
+        if self.translate_state is False:
+            self.translate_state = True
+            self.translateBtn.setText('结束翻译')
+            self.translateBtn.setStyleSheet("color: red;")
+        elif self.translate_state is True:
+            self.translate_state = False
+            self.translateBtn.setText('翻译')
+            self.translateBtn.setStyleSheet("color: black;")
 
     def onItemClicked(self, item):
         # 处理项目的选择状态变化

src/windows/res/tools/translate.py

@@ -4,12 +4,13 @@ from hashlib import md5
 import requests
 import json
 
-def translate_text(text, from_lang, to_lang, api_key):
+def translate_text(text, from_lang, to_lang):
     base_url = "https://fanyi-api.baidu.com/api/trans/vip/translate"
 
     def make_md5(s, encoding='utf-8'):
         return md5(s.encode(encoding)).hexdigest()
 
+    api_key = 'LxybddF5Y966dC1BXTc5'
     salt = random.randint(32768, 65536)
     sign = make_md5('20220121001062201' + text + str(salt) + api_key)
 
@@ -28,20 +29,3 @@ def translate_text(text, from_lang, to_lang, api_key):
         return translated_text
     else:
         return '翻译失败'
-
-# 输入要翻译的文本
-text_to_translate = input("请输入要翻译的文本：")
-
-# 设置源语言和目标语言
-from_language = 'auto'  # 自动检测语言
-to_language = 'zh'  # 翻译成中文
-
-# 替换为您的API密钥
-api_key = 'LxybddF5Y966dC1BXTc5'
-
-# 调用翻译函数并输出结果
-translated_text = translate_text(text_to_translate, from_language, to_language, api_key)
-if translated_text:
-    print("翻译结果：", translated_text)
-else:
-    print("翻译失败。")

src/windows/translate_window.py

@@ -1,4 +1,6 @@
-from PyQt5.QtWidgets import QMainWindow
+from PyQt5.QtCore import Qt
+from PyQt5.QtGui import QTextOption
+from PyQt5.QtWidgets import QMainWindow, QTextBrowser, QSizePolicy
 from PyQt5 import uic
 
 
@@ -7,3 +9,18 @@ class translate(QMainWindow):
         super().__init__()
         # 创建一个新窗口并加载translate.ui文件
         uic.loadUi('res/ui/translate_window.ui', self)
+
+        self.orgwords = self.findChild(QTextBrowser, 'orgwords')
+        self.transwords = self.findChild(QTextBrowser, 'transwords')
+
+        # 设置文本自动换行
+        self.orgwords.setWordWrapMode(QTextOption.WrapAtWordBoundaryOrAnywhere)
+        self.transwords.setWordWrapMode(QTextOption.WrapAtWordBoundaryOrAnywhere)
+
+        # 居中显示文本
+        self.orgwords.setAlignment(Qt.AlignCenter)
+        self.transwords.setAlignment(Qt.AlignCenter)
+
+        # 设置文本自适应大小
+        self.orgwords.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+        self.transwords.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)