diff --git a/src/.idea/misc.xml b/src/.idea/misc.xml index 59b947d..9ed2f00 100644 --- a/src/.idea/misc.xml +++ b/src/.idea/misc.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/src/.idea/src.iml b/src/.idea/src.iml index b50f66a..b22c18f 100644 --- a/src/.idea/src.iml +++ b/src/.idea/src.iml @@ -1,8 +1,10 @@ - - + + + + diff --git a/src/windows/__pycache__/translate_window.cpython-311.pyc b/src/windows/__pycache__/translate_window.cpython-311.pyc index e621944..8099c7d 100644 Binary files a/src/windows/__pycache__/translate_window.cpython-311.pyc and b/src/windows/__pycache__/translate_window.cpython-311.pyc differ diff --git a/src/windows/control/controltools/__pycache__/regPictrue.cpython-311.pyc b/src/windows/control/controltools/__pycache__/regPictrue.cpython-311.pyc new file mode 100644 index 0000000..3fb6944 Binary files /dev/null and b/src/windows/control/controltools/__pycache__/regPictrue.cpython-311.pyc differ diff --git a/src/windows/control/controltools/__pycache__/setShotArea.cpython-311.pyc b/src/windows/control/controltools/__pycache__/setShotArea.cpython-311.pyc new file mode 100644 index 0000000..9635fc9 Binary files /dev/null and b/src/windows/control/controltools/__pycache__/setShotArea.cpython-311.pyc differ diff --git a/src/windows/control/controltools/contest(1).py b/src/windows/control/controltools/contest(1).py new file mode 100644 index 0000000..b1567eb --- /dev/null +++ b/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() diff --git a/src/windows/control/controltools/contest.py b/src/windows/control/controltools/contest.py new file mode 100644 index 0000000..f8251a6 --- /dev/null +++ b/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() diff --git a/src/windows/control/controltools/regPictrue.py b/src/windows/control/controltools/regPictrue.py new file mode 100644 index 0000000..c309c1c --- /dev/null +++ b/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) diff --git a/src/windows/control/contrltools/setShotArea.py b/src/windows/control/controltools/setShotArea.py similarity index 71% rename from src/windows/control/contrltools/setShotArea.py rename to src/windows/control/controltools/setShotArea.py index 52ff704..d51854e 100644 --- a/src/windows/control/contrltools/setShotArea.py +++ b/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() diff --git a/src/windows/control/database/databaseconnact.py b/src/windows/control/database/databaseconnact.py index db76aa5..5dfcd83 100644 --- a/src/windows/control/database/databaseconnact.py +++ b/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() diff --git a/src/windows/main_window.py b/src/windows/main_window.py index 9f8a750..4e93311 100644 --- a/src/windows/main_window.py +++ b/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): # 处理项目的选择状态变化 diff --git a/src/windows/res/tools/__pycache__/translate.cpython-311.pyc b/src/windows/res/tools/__pycache__/translate.cpython-311.pyc new file mode 100644 index 0000000..17b6e72 Binary files /dev/null and b/src/windows/res/tools/__pycache__/translate.cpython-311.pyc differ diff --git a/src/windows/res/tools/translate.py b/src/windows/res/tools/translate.py index 5aee742..548172b 100644 --- a/src/windows/res/tools/translate.py +++ b/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("翻译失败。") diff --git a/src/windows/translate_window.py b/src/windows/translate_window.py index c433996..0ee44a9 100644 --- a/src/windows/translate_window.py +++ b/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)