From 35e8cc9858eb261b68c6771d6893c81173368475 Mon Sep 17 00:00:00 2001
From: havoc420ubuntu <2993167370@qq.com>
Date: Fri, 16 May 2025 11:35:53 +0000
Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=8F=20v0.1=20base=20task-1?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 base_move/move_base_hori_line.py | 23 ++++++++-----
 task_1/task_1.py                 | 56 ++++++++++++++++----------------
 2 files changed, 42 insertions(+), 37 deletions(-)

diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py
index ebf3003..60ec019 100644
--- a/base_move/move_base_hori_line.py
+++ b/base_move/move_base_hori_line.py
@@ -327,7 +327,8 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
 def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, 
                               pass_align=False,
                               scan_qrcode=False, qr_check_interval=0.3, 
-                              observe=False,):
+                              observe=False,
+                              bad_big_angle_corret=False):
     """
     对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
     参数:
@@ -340,6 +341,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
         observe: 是否打印调试信息
         scan_qrcode: 是否在旋转过程中扫描QR码，默认为False
         qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒
+
+        bad_big_angle_corret: 是否对大角度错误进行修正，默认为False # TODO clear
     返回:
         bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作；
                   如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果)
@@ -468,13 +471,12 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
     start_position = list(ctrl.odo_msg.xyz)
     
     # 在起点放置蓝色标记
-    if observe and hasattr(ctrl, 'place_marker'):
-        # 获取当前姿态
-        yaw = ctrl.odo_msg.rpy[2]
-        ctrl.place_marker(start_position[0], start_position[1], 
-                        start_position[2] if len(start_position) > 2 else 0.0, 
-                        'blue', observe=True)
-        print(f"在起点位置放置蓝色标记: {start_position}")
+    # TEST 获取当前姿态
+    yaw = ctrl.odo_msg.rpy[2]
+    ctrl.place_marker(start_position[0], start_position[1], 
+                    start_position[2] if len(start_position) > 2 else 0.0, 
+                    'blue', observe=True)
+    print(f"在起点位置放置蓝色标记: {start_position}")
     
     # 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
     # 圆心与机器人当前位置的距离就是圆弧半径
@@ -729,7 +731,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
     
     final_angle_deg = math.degrees(final_angle_turned)
     # 这里使用原始目标角度进行比较
-    target_angle_deg = angle_deg if left else -angle_deg
+    if not bad_big_angle_corret:
+        target_angle_deg = angle_deg if left else -angle_deg
+    else:
+        target_angle_deg = -angle_deg if left else angle_deg
     
     if observe:
         # 输出停止过程中角度变化
diff --git a/task_1/task_1.py b/task_1/task_1.py
index c65b8e7..67d5253 100644
--- a/task_1/task_1.py
+++ b/task_1/task_1.py
@@ -9,7 +9,7 @@ from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hor
 from utils.detect_track import detect_horizontal_track_edge
 from base_move.move_base_hori_line import calculate_distance_to_line
 
-observe = True
+observe = False
 
 def run_task_1(ctrl, msg):
     print('🐶 Running task 1...')
@@ -17,43 +17,43 @@ def run_task_1(ctrl, msg):
     # v2
     print('😺 task 1 - 1')
     # 在 arc_turn_around_hori_line 中启用 QR 码扫描
-    # turn_success, qr_result = arc_turn_around_hori_line(
-    #     ctrl=ctrl,
-    #     msg=msg,
-    #     angle_deg=85,
-    #     left=False,
-    #     observe=observe,
-    #     scan_qrcode=True,  # 启用 QR 码扫描
-    #     qr_check_interval=0.3  # 每 0.3 秒检查一次扫描结果
-    # )
-    # return
+    turn_success, qr_result = arc_turn_around_hori_line(
+        ctrl=ctrl,
+        msg=msg,
+        angle_deg=85,
+        left=False,
+        observe=observe,
+        scan_qrcode=True,  # 启用 QR 码扫描
+        qr_check_interval=0.3  # 每 0.3 秒检查一次扫描结果
+    )
     
-    # if qr_result:
-    #     print(f"🎯 在任务 1-1 中成功扫描到 QR 码: {qr_result}")
-    # else:
-    #     print("⚠️ 任务 1-1 中未能扫描到任何 QR 码")
+    if qr_result:
+        print(f"🎯 在任务 1-1 中成功扫描到 QR 码: {qr_result}")
+    else:
+        print("⚠️ 任务 1-1 中未能扫描到任何 QR 码")
 
-    # print('😺 task 1 - 2')
-    # # 执行常规的移动操作，不需要 QR 码扫描
-    # move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)
+    print('😺 task 1 - 2')
+    # 执行常规的移动操作，不需要 QR 码扫描
+    move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)
 
     print('😺 task 1 - 3')
-    qr_result = "A-1"
-    direction = True if qr_result == 'A-1' else False
+    # direction = True if qr_result == 'A-1' else False
+    # TODO 
     turn_success = arc_turn_around_hori_line(
         ctrl=ctrl,
         msg=msg,
-        angle_deg=180,
-        target_distance=0.5,
-        left=direction,
-        # pass_align=True,
+        angle_deg=170,
+        target_distance=0.4,  # TODO 优化这里的参数
+        left=False, # direction,
+        pass_align=True,
         observe=observe,
+
+        # TODO clear
+        bad_big_angle_corret=True
     )
-    
-    return
 
     print('😺 task 1 - 4')
-    move_to_hori_line(ctrl, msg, observe=observe)
-
+    # move_to_hori_line(ctrl, msg, target_distance=0.6, observe=observe)
 
+    print('😺 task 1 - 5 休眠，模拟装货')