feat(base_move): improve arc_turn_around_hori_line function

- Add target_distance parameter to adjust the radius of the arc
- Modify the function to subtract target_distance from the calculated radius
- Update the velocity calculation to use the absolute value
- Increase the duration by

base_move/move_base_hori_line.py

@@ -352,7 +352,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
     # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断
     return abs(distance_moved - abs(distance_to_move)) < 0.1  # 如果误差小于10厘米，则认为成功
 
-def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False):
+def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left=True, observe=False):
     """
     对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
     参数:
@@ -366,20 +366,24 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False)
     """
     # 1. 对准横线
     print("校准到横向线水平")
-    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
+    aligned = align_to_horizontal_line(ctrl, msg, observe=False)
     if not aligned:
         print("无法校准到横向线水平，停止动作")
         return False
 
     # 2. 检测横线并计算距离
     image = ctrl.image_processor.get_current_image()
-    edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe)
+    edge_point, edge_info = detect_horizontal_track_edge(image, observe=False)
     if edge_point is None or edge_info is None:
         print("无法检测到横向线，停止动作")
         return False
 
     camera_height = 0.355  # 单位: 米
     r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
+
+    # 减去目标距离
+    r -= target_distance
+
     if r is None:
         print("无法计算到横向线的距离，停止动作")
         return False
@@ -391,7 +395,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False)
     angle_rad = math.radians(angle_deg)
     # 设定角速度（rad/s），可根据实际调整
     w = 0.4 if left else -0.4  # 左转为正，右转为负
-    v = w * r  # 线速度
+    v = abs(w * r)  # 线速度
     t = abs(angle_rad / w)  # 运动时间
 
     if observe:
@@ -401,7 +405,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False)
     msg.mode = 11
     msg.gait_id = 26
     msg.vel_des = [v, 0, w]  # [前进速度, 侧向速度, 角速度]
-    msg.duration = int((t + 1) * 1000)  # 加1秒余量
+    msg.duration = int((t + 2) * 1000)  # 加1秒余量
     msg.step_height = [0.06, 0.06]
     msg.life_count += 1
 

task_1/task_1.py

@@ -5,7 +5,7 @@ import os
 
 # 添加父目录到路径，以便能够导入utils
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
-from base_move.move_base_hori_line import align_to_horizontal_line, move_to_hori_line
+from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line
 
 observe = True
 
@@ -13,7 +13,8 @@ def run_task_1(ctrl, msg):
     print('Running task 1...')
 
     # v2
-    move_to_hori_line(ctrl, msg, observe=observe)
+    arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe)
+    # move_to_hori_line(ctrl, msg, observe=observe)