test(task_1): adjust go_straight parameters and comment out angle correction

- Comment out angle correction logic in go_straight function
- Adjust go_straight parameters in task_1
- Add success message for horizontal line calibration
- Update main function to include task_test
- Modify marker request response format

base_move/go_straight.py

@@ -134,24 +134,24 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
                 ctrl.Send_cmd(msg)
             
             # 如果偏离初始方向，进行角度修正
-            if abs(yaw_error) > angle_correction_threshold:
+            # if abs(yaw_error) > angle_correction_threshold:
-                # 计算角速度修正值，偏差越大修正越强
+            #     # 计算角速度修正值，偏差越大修正越强
-                angular_correction = -yaw_error * 0.5  # 比例系数可调整
+            #     angular_correction = -yaw_error * 0.5  # 比例系数可调整
-                # 限制最大角速度修正
+            #     # 限制最大角速度修正
-                angular_correction = max(min(angular_correction, 0.2), -0.2)
+            #     angular_correction = max(min(angular_correction, 0.2), -0.2)
                 
-                if observe and abs(angular_correction) > 0.05:
+            #     if observe and abs(angular_correction) > 0.05:
-                    warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度")
+            #         warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度")
                 
-                # 应用角速度修正
+            #     # 应用角速度修正
-                msg.vel_des[2] = angular_correction
+            #     msg.vel_des[2] = angular_correction
-                msg.life_count += 1
+            #     msg.life_count += 1
-                ctrl.Send_cmd(msg)
+            #     ctrl.Send_cmd(msg)
-            elif msg.vel_des[2] != 0:
+            # elif msg.vel_des[2] != 0:
-                # 如果方向已修正，重置角速度
+            #     # 如果方向已修正，重置角速度
-                msg.vel_des[2] = 0
+            #     msg.vel_des[2] = 0
-                msg.life_count += 1
+            #     msg.life_count += 1
-                ctrl.Send_cmd(msg)
+            #     ctrl.Send_cmd(msg)
             
             if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval):
                 debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离")

base_move/move_base_hori_line.py

@@ -296,6 +296,8 @@ def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
     if not aligned:
         error("无法校准到横向线水平，停止移动", "停止")
         return False
+    else:
+        success("校准完成", "完成")
     
     go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe)
 

main.py

@@ -23,6 +23,8 @@ from utils.base_msg import BaseMsg
 from task_1.task_1 import run_task_1
 from task_5.task_5 import run_task_5
 
+from task_test.task_test import run_task_test
+
 pass_marker = True
 
 def main():
@@ -34,13 +36,15 @@ def main():
     try:
         print("Recovery stand")
         Ctrl.base_msg.stand_up()
+        Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat
         
         # time.sleep(100) # TEST 
-
         run_task_1(Ctrl, msg)
 
         # run_task_5(Ctrl, msg)
 
+        # run_task_test(Ctrl, msg)
+
         # time.sleep(100)
 
     except KeyboardInterrupt:
@@ -104,14 +108,14 @@ class Robot_Ctrl(object):
             self.calibration_offset = self.odo_msg.xyz
             self.is_calibrated = True
             print(f"校准完成，基准值: {self.calibration_offset}")
-        # 将接收到的 odo 数据减去校准基准值
+        # # 将接收到的 odo 数据减去校准基准值
         self.odo_msg.xyz = [
             self.odo_msg.xyz[0] - self.calibration_offset[0],
             self.odo_msg.xyz[1] - self.calibration_offset[1],
             self.odo_msg.xyz[2] - self.calibration_offset[2]
         ]
-        # if self.odo_msg.timestamp % 100 == 0:
+        # if self.odo_msg.timestamp % 50 == 0:
-        #     print(self.odo_msg.xyz)
+        #     print(self.odo_msg.rpy)
 
     def odo_reset(self):
         self.calibration_offset = self.odo_msg.xyz

task_1/task_1.py

@@ -25,9 +25,10 @@ def run_task_1(ctrl, msg):
     info('开始执行任务1...', "启动")
  
     # TEST better align
-    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
+    # aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
-    print(aligned)
+    # print(aligned)
-    return
+    # go_straight(ctrl, msg, -10, observe=observe)
+    # return
 
     # v2
     section('任务1-1：转弯并扫描QR码', "移动")
@@ -57,7 +58,7 @@ def run_task_1(ctrl, msg):
         ctrl=ctrl,
         msg=msg,
         angle_deg=-170,  # 负值表示右转
-        target_distance=0.5,
+        target_distance=0.8,
         #
         pass_align=True,
         observe=observe,
@@ -72,11 +73,11 @@ def run_task_1(ctrl, msg):
         return
     
     section('任务1-4：直线移动', "移动")
-    move_distance = 0.3
+    move_distance = 0.5
     move_speed = 0.5
     if not direction:
         # TODO to check
-        go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
+        go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_distance, observe=observe)
     else:
         go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
 
@@ -92,7 +93,7 @@ def run_task_1(ctrl, msg):
     ctrl.base_msg.stand_up()
 
     section('任务1-6：返回', "移动")
-    go_straight(ctrl, msg, distance=-(move_distance + res['radius'] * 2), speed=0.3, observe=observe)
+    go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.2), speed=0.3, observe=observe)
 
     # turn and back
     turn_degree(ctrl, msg, degree=-90)
@@ -102,7 +103,7 @@ def run_task_1(ctrl, msg):
         ctrl=ctrl,
         msg=msg,
         radius=res['radius'] * 2,
-        angle_deg=90,
+        angle_deg=85,
         #
         pass_align=True,
         observe=observe

task_test/task_test.py

@@ -0,0 +1,15 @@
+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)
+    

utils/marker_client.py

@@ -36,7 +36,10 @@ class MarkerRunner:
     
     def send_request(self, x, y, z, color, observe=False):
         if self.pass_flag:
-            return "pass flag."
+            return {
+                'success': True,
+                'message': "Pass"
+            }
         response = self.marker_client.send_request(x, y, z, color)
         if observe:
             self.marker_client.get_logger().info(