Merge branch 'main' of ssh://120.27.199.238:222/Havoc420mac/mi-task into main

base_move/move_base_hori_line.py

@@ -352,6 +352,77 @@ 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):
+    """
+    对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
+    参数:
+        ctrl: Robot_Ctrl对象
+        msg: robot_control_cmd_lcmt对象
+        angle_deg: 旋转角度，支持90或180度
+        left: True为左转，False为右转
+        observe: 是否打印调试信息
+    返回:
+        bool: 是否成功完成动作
+    """
+    # 1. 对准横线
+    print("校准到横向线水平")
+    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
+    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)
+    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)
+    if r is None:
+        print("无法计算到横向线的距离，停止动作")
+        return False
+
+    if observe:
+        print(f"当前距离: {r:.3f}米")
+
+    # 3. 计算圆弧运动参数
+    angle_rad = math.radians(angle_deg)
+    # 设定角速度（rad/s），可根据实际调整
+    w = 0.4 if left else -0.4  # 左转为正，右转为负
+    v = w * r  # 线速度
+    t = abs(angle_rad / w)  # 运动时间
+
+    if observe:
+        print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s, 运动时间: {t:.2f}s")
+
+    # 4. 发送圆弧运动指令
+    msg.mode = 11
+    msg.gait_id = 26
+    msg.vel_des = [v, 0, w]  # [前进速度, 侧向速度, 角速度]
+    msg.duration = int((t + 1) * 1000)  # 加1秒余量
+    msg.step_height = [0.06, 0.06]
+    msg.life_count += 1
+
+    ctrl.Send_cmd(msg)
+    time.sleep(t)  # 等待运动完成
+
+    # 5. 发送停止指令
+    msg.mode = 0
+    msg.gait_id = 0
+    msg.duration = 0
+    ctrl.Send_cmd(msg)
+
+    if observe:
+        print("圆弧运动完成")
+
+    return True
+
 # 用法示例
 if __name__ == "__main__":
     move_to_hori_line(None, None, observe=True)
+    # 90度左转
+    arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True)
+    # 180度右转
+    arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)