191 lines
7.4 KiB
Python
191 lines
7.4 KiB
Python
import math
|
||
import time
|
||
import sys
|
||
import os
|
||
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("旋转控制")
|
||
|
||
def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False):
|
||
"""
|
||
结合里程计实现精确稳定的旋转指定角度
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
|
||
absolute: 是否绝对角度，默认为 False
|
||
precision: 是否启用高精度模式，该模式下会使用更慢的速度和更细致的微调，默认为 False
|
||
|
||
返回:
|
||
Bool: 是否成功旋转到指定角度
|
||
"""
|
||
# 将角度转换为弧度
|
||
target_rad = math.radians(degree)
|
||
|
||
# 获取当前朝向
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
# 计算目标朝向
|
||
if absolute:
|
||
target_yaw = target_rad
|
||
else:
|
||
target_yaw = current_yaw + target_rad
|
||
|
||
# 标准化目标角度到 [-pi, pi] 范围
|
||
if target_yaw > math.pi:
|
||
target_yaw -= 2 * math.pi
|
||
if target_yaw < -math.pi:
|
||
target_yaw += 2 * math.pi
|
||
|
||
# 定义允许的误差范围（弧度）
|
||
# 如果是精确模式，使用更小的误差阈值
|
||
limit = 0.03 if precision else 0.04 # 约1.7度或2.3度
|
||
|
||
# 确定最短旋转方向
|
||
remaining_dist = target_yaw - current_yaw
|
||
if remaining_dist > math.pi:
|
||
remaining_dist -= 2 * math.pi
|
||
elif remaining_dist < -math.pi:
|
||
remaining_dist += 2 * math.pi
|
||
|
||
# 如果误差已经在允许范围内，无需旋转
|
||
if abs(remaining_dist) <= limit:
|
||
success(f"当前角度误差已在允许范围内，无需旋转", "成功")
|
||
return True
|
||
|
||
# 确定旋转方向和速度
|
||
turn_direction = 1 if remaining_dist > 0 else -1
|
||
|
||
# 根据精确模式选择旋转速度
|
||
if precision:
|
||
# 精确模式下，角度较大时使用中等速度，较小时使用慢速
|
||
if abs(remaining_dist) > 0.5: # 大约28度以上
|
||
turn_speed = 0.25
|
||
else:
|
||
turn_speed = 0.15
|
||
else:
|
||
# 非精确模式下，角度较大时使用快速，较小时使用中等速度
|
||
if abs(remaining_dist) > 0.5: # 大约28度以上
|
||
turn_speed = 0.5
|
||
else:
|
||
turn_speed = 0.3
|
||
|
||
info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°, 需旋转{math.degrees(abs(remaining_dist)):.2f}°", "旋转")
|
||
|
||
# 发送旋转命令（持续旋转）
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.vel_des = [0, 0, turn_speed * turn_direction] # 转向速度和方向
|
||
msg.duration = 30000 # 设置一个大的持续时间值，后面会主动停止
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"开始持续旋转：方向={'逆时针' if turn_direction > 0 else '顺时针'}, 速度={turn_speed}", "旋转")
|
||
|
||
# 等待并监测旋转进度
|
||
start_time = time.time()
|
||
last_yaw = current_yaw
|
||
stable_count = 0
|
||
|
||
# 设置最大超时时间（秒）防止无限等待
|
||
max_timeout = 15 if abs(remaining_dist) > 1.0 else 10
|
||
|
||
while True:
|
||
time.sleep(0.05) # 高频监测旋转状态
|
||
|
||
# 获取当前角度
|
||
current_yaw_now = ctrl.odo_msg.rpy[2]
|
||
|
||
# 计算当前与目标的角度差
|
||
current_error = target_yaw - current_yaw_now
|
||
if current_error > math.pi:
|
||
current_error -= 2 * math.pi
|
||
elif current_error < -math.pi:
|
||
current_error += 2 * math.pi
|
||
|
||
# 计算已旋转角度（用于检测机器人是否在移动）
|
||
rotated = current_yaw_now - last_yaw
|
||
if abs(rotated) > math.pi:
|
||
rotated = 2 * math.pi - abs(rotated)
|
||
rotated *= -1 if current_yaw_now > last_yaw else 1
|
||
|
||
last_yaw = current_yaw_now
|
||
|
||
# 每0.5秒打印一次进度
|
||
elapsed = time.time() - start_time
|
||
if int(elapsed * 2) % 10 == 0: # 每5秒
|
||
debug(f"旋转进度: {elapsed:.1f}s, 剩余角度: {math.degrees(abs(current_error)):.2f}°", "进度")
|
||
|
||
# 判断是否已经接近目标角度
|
||
if abs(current_error) <= limit:
|
||
# 立即发送停止命令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [0, 0, 0] # 停止旋转
|
||
msg.duration = 200 # 短暂停止命令
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
success(f"旋转成功，误差在允许范围内: {math.degrees(abs(current_error)):.2f}°", "成功")
|
||
return True
|
||
|
||
# 检测是否接近目标角度，需要减速
|
||
if abs(current_error) < 0.3 and abs(turn_speed) > 0.15: # 约17度内减速
|
||
turn_speed = 0.15 * (1 if turn_speed > 0 else -1)
|
||
msg.vel_des = [0, 0, turn_speed]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"接近目标，减速至: {turn_speed}", "减速")
|
||
|
||
# 如果旋转方向不对（过头或方向错误），修正方向
|
||
if (current_error > 0 and turn_direction < 0) or (current_error < 0 and turn_direction > 0):
|
||
turn_direction = 1 if current_error > 0 else -1
|
||
turn_speed = 0.15 # 方向调整时使用低速
|
||
msg.vel_des = [0, 0, turn_speed * turn_direction]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"调整旋转方向: {'逆时针' if turn_direction > 0 else '顺时针'}, 速度={turn_speed}", "方向")
|
||
|
||
# 检查超时
|
||
if elapsed > max_timeout:
|
||
# 发送停止命令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [0, 0, 0] # 停止旋转
|
||
msg.duration = 500
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
error(f"旋转超时，已停止。当前误差: {math.degrees(abs(current_error)):.2f}°", "超时")
|
||
return False
|
||
|
||
# 检测机器人是否卡住（长时间无明显角度变化）
|
||
if abs(rotated) < 0.005: # 角度变化很小
|
||
stable_count += 1
|
||
else:
|
||
stable_count = 0
|
||
|
||
# 如果连续多次检测到角度几乎不变，可能卡住了
|
||
if stable_count > 30: # 连续30次检测（约1.5秒）角度几乎不变
|
||
# 尝试增加速度或改变方向
|
||
if abs(turn_speed) < 0.5:
|
||
turn_speed = 0.5 * turn_direction
|
||
msg.vel_des = [0, 0, turn_speed]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"检测到旋转停滞，增加速度至: {turn_speed}", "调整")
|
||
stable_count = 0
|
||
else:
|
||
# 如果速度已经很大还是卡住，可能有障碍，停止旋转
|
||
msg.vel_des = [0, 0, 0] # 停止旋转
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
error(f"检测到旋转异常，已停止。当前误差: {math.degrees(abs(current_error)):.2f}°", "异常")
|
||
return False
|