修改代码

master
abc15379 2 years ago
parent 9d599f49dc
commit 5062383ba4

@ -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>

@ -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">

@ -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()

@ -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()

@ -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)

@ -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()

@ -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()

@ -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):
# 处理项目的选择状态变化

@ -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("翻译失败。")

@ -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)

Loading…
Cancel
Save