diff --git a/base_move/turn_degree.py b/base_move/turn_degree.py index 19e9ca2..6d14cd3 100644 --- a/base_move/turn_degree.py +++ b/base_move/turn_degree.py @@ -43,7 +43,7 @@ def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False): # 定义允许的误差范围(弧度) # 如果是精确模式,使用更小的误差阈值 - limit = 0.03 if precision else 0.04 # 约1.7度或2.3度 + limit = 0.05 # 计算最短旋转方向和距离 def circle_dist(target, location): @@ -94,7 +94,7 @@ def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False): debug(f"等待旋转完成: {wait_time:.2f}秒", "时间") # 精确模式下使用实时监控而不是固定等待时间 - if precision and abs(dist) > 0.1: # 对于较大角度 + if abs(dist) > 0.1: # 对于较大角度 start_time = time.time() last_yaw = current_yaw stable_count = 0 diff --git a/main.py b/main.py index 595143a..190472b 100644 --- a/main.py +++ b/main.py @@ -22,9 +22,8 @@ from utils.base_msg import BaseMsg from task_1.task_1 import run_task_1 from task_2_5.task_2_5 import run_task_2_5 -from task_5.task_5 import run_task_5 - -from task_test.task_test import run_task_test +from task_4.task_4 import run_task_4 +from task_test.task_left_line import run_task_test pass_marker = True @@ -42,11 +41,11 @@ def main(): # time.sleep(100) # TEST # run_task_1(Ctrl, msg) - run_task_2_5(Ctrl, msg) + # run_task_2_5(Ctrl, msg) - # run_task_5(Ctrl, msg) + # run_task_4(Ctrl, msg) - # run_task_test(Ctrl, msg) + run_task_test(Ctrl, msg) # time.sleep(100) diff --git a/task_2_5/task_2_5.py b/task_2_5/task_2_5.py index bd28369..61dc58c 100644 --- a/task_2_5/task_2_5.py +++ b/task_2_5/task_2_5.py @@ -11,41 +11,36 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin observe = True -def run_task_2_5(Ctrl, msg): +def run_task_2_5(Ctrl, msg, direction='left'): section('任务2.5:预备进入任务3', "启动") # TEST turn_degree(Ctrl, msg, 90, absolute=observe) - align_to_horizontal_line( - Ctrl, - msg, - detect_func_version=2, - observe=observe, - ) - - return - section('任务2.5-1:第一次旋转', "移动") - arc_turn_around_hori_line( + turn_success, res = arc_turn_around_hori_line( Ctrl, msg, - angle_deg=-90, - target_distance=0.5, + angle_deg=90 if direction == 'left' else -90, + target_distance=0.3, detect_func_version=2, observe=observe, ) section('任务2.5-2:第二次旋转', "移动") - arc_turn_around_hori_line( - Ctrl, - msg, - angle_deg=90, - target_distance=0.5, - detect_func_version=2, - observe=observe, - ) + turn_degree(Ctrl, msg, degree=90, absolute=True) + + # arc_turn_around_hori_line( + # Ctrl, + # msg, + # angle_deg=70, + # # target_distance=0.5, + # pass_align=True, + # radius=res['radius'], + # detect_func_version=2, + # observe=observe, + # ) \ No newline at end of file diff --git a/task_4/task_4.py b/task_4/task_4.py new file mode 100644 index 0000000..4efd29c --- /dev/null +++ b/task_4/task_4.py @@ -0,0 +1,28 @@ +import time +import sys +import os + +# 添加父目录到路径,以便能够导入utils +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from base_move.turn_degree import turn_degree +from base_move.go_straight import go_straight +from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing + +# 创建本模块特定的日志记录器 +logger = get_logger("任务5") + +def run_task_4(ctrl, msg): + """ + 参数: + ctrl: Robot_Ctrl对象 + msg: 控制消息对象 + image_processor: 可选的图像处理器实例 + """ + + turn_degree(ctrl, msg, 90, absolute=90) + + time.sleep(100) + + # go_straight(ctrl, msg, distance=6) + diff --git a/task_5/detect_arrow_direction.py b/task_test/detect_arrow_direction.py similarity index 100% rename from task_5/detect_arrow_direction.py rename to task_test/detect_arrow_direction.py diff --git a/task_5/task_5.py b/task_test/task_arrow.py similarity index 96% rename from task_5/task_5.py rename to task_test/task_arrow.py index 01aad70..aec7da8 100644 --- a/task_5/task_5.py +++ b/task_test/task_arrow.py @@ -5,7 +5,7 @@ import os # 添加父目录到路径,以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from task_5.detect_arrow_direction import ArrowDetector +from task_test.detect_arrow_direction import ArrowDetector from base_move.turn_degree import turn_degree from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing diff --git a/task_test/task_left_line.py b/task_test/task_left_line.py new file mode 100644 index 0000000..8183850 --- /dev/null +++ b/task_test/task_left_line.py @@ -0,0 +1,17 @@ +import time +import sys +import os + +# 添加父目录到路径,以便能够导入utils +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from base_move.turn_degree import turn_degree +from base_move.move_base_hori_line import follow_left_side_track +from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing + +# 创建本模块特定的日志记录器 +logger = get_logger("任务-test") + +def run_task_test(ctrl, msg): + # follow_left_side_track(ctrl, msg) + time.sleep(100) \ No newline at end of file diff --git a/task_test/task_test.py b/task_test/task_test.py deleted file mode 100644 index eec073a..0000000 --- a/task_test/task_test.py +++ /dev/null @@ -1,15 +0,0 @@ -import time -import sys -import os - -# 添加父目录到路径,以便能够导入utils -sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) - -from task_5.detect_arrow_direction import ArrowDetector -from base_move.turn_degree import turn_degree - -def run_task_test(ctrl, msg): - ctrl.base_msg.stop() - turn_degree(ctrl, msg, 90) - turn_degree(ctrl, msg, 90) - diff --git a/test/ros2/rgb-camera/img-raw-get.py b/test/ros2/rgb-camera/img-raw-get.py index ddf8230..edadf21 100644 --- a/test/ros2/rgb-camera/img-raw-get.py +++ b/test/ros2/rgb-camera/img-raw-get.py @@ -51,7 +51,7 @@ class ImageSubscriber(Node): if self.cv_image is not None: # Generate a timestamped filename timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - filename = f"captured_images/path/image_{timestamp}.png" + filename = f"./captured_images/left_path/image_{timestamp}.png" cv2.imwrite(filename, self.cv_image) self.get_logger().info(f"Saved image as {filename}") diff --git a/utils/detect_track.py b/utils/detect_track.py index b9f715e..88163b0 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -19,7 +19,7 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True edge_point: 赛道前方边缘点的坐标 (x, y) edge_info: 边缘信息字典 """ - # observe = False # TEST + observe = False # TEST # 如果输入是字符串(文件路径),则加载图像 if isinstance(image, str): img = cv2.imread(image) @@ -361,7 +361,7 @@ def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=T edge_point: 赛道前方边缘点的坐标 (x, y) edge_info: 边缘信息字典 """ - # observe = False # TEST + observe = False # TEST # 如果输入是字符串(文件路径),则加载图像 if isinstance(image, str): img = cv2.imread(image)