From 116fd15b5613522bfbe2350ed7ae93cc38fe8ed7 Mon Sep 17 00:00:00 2001
From: Havoc <2993167370@qq.com>
Date: Thu, 15 May 2025 16:51:50 +0800
Subject: [PATCH] =?UTF-8?q?fix(base=5Fmove):=20=E4=BF=AE=E6=AD=A3=20arc=5F?=
 =?UTF-8?q?turn=5Faround=5Fhori=5Fline=20=E5=87=BD=E6=95=B0=E4=B8=AD?=
 =?UTF-8?q?=E7=9A=84=E8=88=AA=E5=90=91=E8=A7=92=E8=8E=B7=E5=8F=96=E6=96=B9?=
 =?UTF-8?q?=E5=BC=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 将航向角的获取方式从 ctrl.odo_msg.yaw 修改为 ctrl.odo_msg.rpy[2]
- 增加航向角的归一化处理逻辑，确保角度在 -π 到 π 之间
---
 base_move/move_base_hori_line.py | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py
index 3087286..ea40cb1 100644
--- a/base_move/move_base_hori_line.py
+++ b/base_move/move_base_hori_line.py
@@ -409,7 +409,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
     ctrl.Send_cmd(msg)
 
     # 记录起始角度和位置
-    start_yaw = ctrl.odo_msg.yaw
+    start_yaw = ctrl.odo_msg.rpy[2]
     start_position = list(ctrl.odo_msg.xyz)
 
     # 计算圆弧长度
@@ -428,10 +428,13 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
         dy = current_position[1] - start_position[1]
         distance_moved = math.sqrt(dx*dx + dy*dy)
         # 计算已旋转角度
-        current_yaw = ctrl.odo_msg.yaw
+        current_yaw = ctrl.odo_msg.rpy[2]
         angle_turned = current_yaw - start_yaw
         # 角度归一化处理
-        # ...
+        while angle_turned > math.pi:
+            angle_turned -= 2 * math.pi
+        while angle_turned < -math.pi:
+            angle_turned += 2 * math.pi
         time.sleep(0.05)
 
     # 5. 发送停止指令