From afdeeec21ff74d0b27639941943268666782e6d4 Mon Sep 17 00:00:00 2001
From: Havoc <2993167370@qq.com>
Date: Thu, 15 May 2025 16:17:24 +0800
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20=E5=A2=9E=E5=8A=A0=E5=9C=86?=
 =?UTF-8?q?=E5=BC=A7=E8=BD=AC=E5=90=91=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 新增 arc_turn_around_hori_line 函数，实现机器人对准横线后进行圆弧转向
- 支持90度和180度的左转或右转，增加调试信息输出
- 优化运动参数计算和指令发送逻辑，确保运动过程的准确性和稳定性
- 在主程序中添加示例调用，展示新功能的使用方法
---
 base_move/move_base_hori_line.py | 71 ++++++++++++++++++++++++++++++++
 1 file changed, 71 insertions(+)

diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py
index 2da6561..684ba09 100644
--- a/base_move/move_base_hori_line.py
+++ b/base_move/move_base_hori_line.py
@@ -352,6 +352,77 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, 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)