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>
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -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)
|
Binary file not shown.
Loading…
Reference in new issue