Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(base_move): 修正 arc_turn_around_hori_line 函数中的航向角获取方式

Browse Source 
- 将航向角的获取方式从 ctrl.odo_msg.yaw 修改为 ctrl.odo_msg.rpy[2]
- 增加航向角的归一化处理逻辑，确保角度在 -π 到 π 之间
This commit is contained in:
Havoc 2025-05-15 16:51:50 +08:00
parent 6779c93888
commit 116fd15b56
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
base_move/move_base_hori_line.py
View File

@ -409,7 +409,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
ctrl.Send_cmd(msg) ctrl.Send_cmd(msg)
# 记录起始角度和位置 # 记录起始角度和位置
start_yaw = ctrl.odo_msg.yaw start_yaw = ctrl.odo_msg.rpy[2]
start_position = list(ctrl.odo_msg.xyz) 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] dy = current_position[1] - start_position[1]
distance_moved = math.sqrt(dx*dx + dy*dy) 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 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) time.sleep(0.05)
# 5. 发送停止指令 # 5. 发送停止指令