havoc420ubuntu
bffcd973e0
- Updated task_1.py to replace turn_degree_twice calls with turn_degree_v2 for improved rotation accuracy. - Modified task_2.py to utilize turn_degree_v2 for enhanced turning capabilities and adjusted movement parameters for better path execution. - Refined go_to_xy parameters in task_2 to optimize navigation and added additional logging for debugging purposes.
643 lines
27 KiB
Python
643 lines
27 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=True):
|
||
"""
|
||
结合里程计实现精确稳定的旋转指定角度
|
||
|
||
参数:
|
||
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]
|
||
info(f"当前角度: {math.degrees(current_yaw):.2f}°", "角度")
|
||
|
||
# 计算目标朝向
|
||
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.05
|
||
|
||
# 计算最短旋转方向和距离
|
||
def circle_dist(target, location):
|
||
value1 = abs(target - location)
|
||
value2 = 2 * math.pi - value1
|
||
direction1 = 1 if target > location else 0 # 1为逆时针，0为顺时针
|
||
|
||
# 计算两个方向哪个距离更短
|
||
if value1 < value2:
|
||
return direction1, value1
|
||
else:
|
||
return 1 - direction1, value2
|
||
|
||
# 获取旋转方向和距离
|
||
direction, dist = circle_dist(target_yaw, current_yaw)
|
||
info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "旋转")
|
||
|
||
if abs(dist) > limit:
|
||
# 主要转向
|
||
const_int = 2470 # 转1.57弧度约需2470的duration值
|
||
|
||
# 精确模式下使用更小的旋转速度
|
||
turn_speed = 0.3 if precision else 0.5
|
||
|
||
# 如果角度很小且使用精确模式，进一步降低速度
|
||
if precision and abs(dist) < 0.2: # 约11.5度
|
||
turn_speed = 0.2
|
||
|
||
# 设置转向命令
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.vel_des = [0, 0, turn_speed if direction > 0 else -turn_speed] # 转向速度
|
||
|
||
# 精确模式下延长转向时间以保证稳定性
|
||
duration_factor = 1.2 if precision else 1.0
|
||
msg.duration = int(const_int * abs(dist) * duration_factor)
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 速度={turn_speed}, 持续时间={msg.duration}", "旋转")
|
||
|
||
# 等待转向完成
|
||
# 精确模式下增加等待时间
|
||
wait_factor = 1.3 if precision else 1.0
|
||
wait_time = 7 * abs(dist) / 1.57 * wait_factor
|
||
debug(f"等待旋转完成: {wait_time:.2f}秒", "时间")
|
||
|
||
# 精确模式下使用实时监控而不是固定等待时间
|
||
if abs(dist) > 0.1: # 对于较大角度
|
||
start_time = time.time()
|
||
last_yaw = current_yaw
|
||
stable_count = 0
|
||
timeout = wait_time + 3 # 增加超时保护
|
||
|
||
# 实时监控旋转进度
|
||
while time.time() - start_time < timeout:
|
||
time.sleep(0.1) # 频繁检查
|
||
|
||
current_yaw_now = ctrl.odo_msg.rpy[2]
|
||
# 计算已旋转角度
|
||
rotated = abs(current_yaw_now - last_yaw)
|
||
if rotated > math.pi:
|
||
rotated = 2 * math.pi - rotated
|
||
|
||
# 如果旋转速度很小（几乎停止）
|
||
if rotated < 0.01: # 约0.6度
|
||
stable_count += 1
|
||
else:
|
||
stable_count = 0
|
||
|
||
last_yaw = current_yaw_now
|
||
|
||
# 如果机器人稳定一段时间，认为旋转完成
|
||
if stable_count >= 5:
|
||
debug(f"检测到旋转已稳定，提前结束等待", "旋转")
|
||
break
|
||
|
||
# 每0.5秒打印一次进度
|
||
elapsed = time.time() - start_time
|
||
if int(elapsed * 2) % 2 == 0:
|
||
remaining_yaw = target_yaw - current_yaw_now
|
||
# 标准化到 [-pi, pi]
|
||
if remaining_yaw > math.pi:
|
||
remaining_yaw -= 2 * math.pi
|
||
if remaining_yaw < -math.pi:
|
||
remaining_yaw += 2 * math.pi
|
||
debug(f"旋转进度: {elapsed:.1f}s/{wait_time:.1f}s, 剩余角度: {math.degrees(remaining_yaw):.2f}°", "进度")
|
||
else:
|
||
# 非精确模式使用固定等待时间
|
||
time.sleep(wait_time)
|
||
|
||
# 获取当前角度
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
info(f"主要转向完成: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度")
|
||
|
||
# 计算剩余误差
|
||
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:
|
||
warning(f"剩余误差: {math.degrees(remaining_dist):.2f}°, 需要进行微调", "角度")
|
||
|
||
# 进行微调
|
||
const_int_tiny = 1200 if not precision else 1000 # 精确模式下使用更小的系数
|
||
|
||
# 精确模式下使用更小的微调速度
|
||
fine_turn_speed = 0.3 if precision else 0.5
|
||
|
||
# 如果剩余误差很小，进一步降低速度
|
||
if abs(remaining_dist) < 0.1: # 约5.7度
|
||
fine_turn_speed = 0.15
|
||
|
||
# 设置微调命令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [0, 0, fine_turn_speed if remaining_dist > 0 else -fine_turn_speed]
|
||
|
||
# 精确模式下使用更长的微调时间
|
||
duration_factor = 1.25 if precision else 1.0
|
||
msg.duration = int(const_int_tiny * abs(remaining_dist) * duration_factor)
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 速度={fine_turn_speed}, 持续时间={msg.duration}", "旋转")
|
||
|
||
# 精确模式下等待更长时间
|
||
wait_time = 5 if not precision else 7
|
||
time.sleep(wait_time)
|
||
|
||
# 获取最终角度
|
||
final_yaw = ctrl.odo_msg.rpy[2]
|
||
info(f"微调完成: 最终角度={math.degrees(final_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度")
|
||
|
||
final_error = abs(target_yaw - final_yaw)
|
||
if final_error > math.pi:
|
||
final_error = 2 * math.pi - final_error
|
||
|
||
# 判断是否成功达到目标
|
||
if final_error <= limit:
|
||
success(f"旋转成功，误差在允许范围内: {math.degrees(final_error):.2f}°", "成功")
|
||
return True
|
||
else:
|
||
warning(f"旋转完成，但误差超出允许范围: {math.degrees(final_error):.2f}° > {math.degrees(limit):.2f}°", "警告")
|
||
|
||
# 在精确模式下，如果误差仍然很大，尝试第二次微调
|
||
if precision and final_error > limit * 2:
|
||
info("尝试第二次微调", "精确")
|
||
|
||
# 计算第二次微调角度
|
||
second_adj = target_yaw - final_yaw
|
||
if second_adj > math.pi:
|
||
second_adj -= 2 * math.pi
|
||
elif second_adj < -math.pi:
|
||
second_adj += 2 * math.pi
|
||
|
||
# 降低微调速度
|
||
very_fine_speed = 0.1
|
||
|
||
# 设置第二次微调命令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [0, 0, very_fine_speed if second_adj > 0 else -very_fine_speed]
|
||
msg.duration = int(800 * abs(second_adj)) # 使用更小的系数
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"第二次微调：旋转{math.degrees(second_adj):.2f}°", "精确")
|
||
|
||
# 等待第二次微调完成
|
||
time.sleep(3)
|
||
|
||
# 获取最终结果
|
||
final_final_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
final_final_error = abs(target_yaw - final_final_yaw)
|
||
if final_final_error > math.pi:
|
||
final_final_error = 2 * math.pi - final_final_error
|
||
|
||
if final_final_error <= limit:
|
||
success(f"第二次微调成功，最终误差: {math.degrees(final_final_error):.2f}°", "成功")
|
||
return True
|
||
else:
|
||
warning(f"两次微调后仍有误差: {math.degrees(final_final_error):.2f}°", "警告")
|
||
# 如果误差已经比第一次小，算作改进
|
||
return final_final_error < final_error
|
||
|
||
return False
|
||
|
||
success("旋转成功完成", "完成")
|
||
return True
|
||
|
||
def turn_degree_twice(ctrl, msg, degree=90, absolute=False, precision=True):
|
||
"""
|
||
结合里程计实现精确稳定的旋转指定角度
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
|
||
absolute: 是否绝对角度，默认为 False
|
||
precision: 是否启用高精度模式，该模式下会使用更慢的速度和更细致的微调，默认为 False
|
||
|
||
返回:
|
||
Bool: 是否成功旋转到指定角度
|
||
"""
|
||
aligned = turn_degree(ctrl, msg, degree=degree, absolute=absolute, precision=precision)
|
||
if not aligned:
|
||
turn_degree(ctrl, msg, degree=degree, absolute=absolute, precision=precision)
|
||
|
||
def turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True):
|
||
"""
|
||
结合里程计实现精确稳定的旋转指定角度，使用rpy[2]实时监控并自动切换到站立姿态
|
||
|
||
参数:
|
||
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]
|
||
info(f"当前角度: {math.degrees(current_yaw):.2f}°", "角度")
|
||
|
||
# 计算目标朝向
|
||
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.05
|
||
|
||
# 计算最短旋转方向和距离
|
||
def circle_dist(target, location):
|
||
value1 = abs(target - location)
|
||
value2 = 2 * math.pi - value1
|
||
direction1 = 1 if target > location else 0 # 1为逆时针，0为顺时针
|
||
|
||
# 计算两个方向哪个距离更短
|
||
if value1 < value2:
|
||
return direction1, value1
|
||
else:
|
||
return 1 - direction1, value2
|
||
|
||
# 获取旋转方向和距离
|
||
direction, dist = circle_dist(target_yaw, current_yaw)
|
||
info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "旋转")
|
||
|
||
if abs(dist) > limit:
|
||
# 主要转向
|
||
|
||
# 精确模式下使用更小的旋转速度
|
||
turn_speed = 0.3 if precision else 0.5
|
||
|
||
# 如果角度很小且使用精确模式，进一步降低速度
|
||
if precision and abs(dist) < 0.2: # 约11.5度
|
||
turn_speed = 0.2
|
||
|
||
# 设置转向命令
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.vel_des = [0, 0, turn_speed if direction > 0 else -turn_speed] # 转向速度
|
||
msg.duration = 0 # 不使用固定时间，而是通过监控角度来停止
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 速度={turn_speed}", "旋转")
|
||
|
||
# 计算预期等待时间（仅用于显示）
|
||
wait_factor = 1.8 if precision else 1.0
|
||
wait_time = 7 * abs(dist) / 1.57 * wait_factor
|
||
debug(f"预计旋转时间: {wait_time:.2f}秒", "时间")
|
||
|
||
# 监控 rpy[2] 来控制旋转停止
|
||
start_time = time.time()
|
||
last_yaw = current_yaw
|
||
stable_count = 0
|
||
timeout = wait_time + 3 # 增加超时保护
|
||
target_reached = False
|
||
|
||
# 实时监控旋转进度
|
||
while time.time() - start_time < timeout:
|
||
time.sleep(0.1) # 频繁检查
|
||
|
||
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
|
||
|
||
# 如果误差在允许范围内，准备停止旋转
|
||
if abs(current_error) <= limit:
|
||
target_reached = True
|
||
debug(f"已达到目标角度: 当前={math.degrees(current_yaw_now):.2f}°, 目标={math.degrees(target_yaw):.2f}°", "旋转")
|
||
break
|
||
|
||
# 计算已旋转角度
|
||
rotated = abs(current_yaw_now - last_yaw)
|
||
if rotated > math.pi:
|
||
rotated = 2 * math.pi - rotated
|
||
|
||
# 如果旋转速度很小（几乎停止）
|
||
if rotated < 0.01: # 约0.6度
|
||
stable_count += 1
|
||
else:
|
||
stable_count = 0
|
||
|
||
last_yaw = current_yaw_now
|
||
|
||
# 如果机器人稳定一段时间，认为旋转完成
|
||
if stable_count >= 10:
|
||
info(f"检测到旋转已稳定，准备停止", "旋转")
|
||
break
|
||
|
||
# 每0.5秒打印一次进度
|
||
elapsed = time.time() - start_time
|
||
if int(elapsed * 2) % 2 == 0:
|
||
info(f"旋转进度: {elapsed:.1f}s/{wait_time:.1f}s, 剩余角度: {math.degrees(current_error):.2f}°", "进度")
|
||
|
||
# 切换到站立姿态
|
||
debug("旋转停止，切换到站立姿态", "姿态")
|
||
msg.mode = 1 # 切换到站立模式
|
||
msg.gait_id = 0
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.duration = 0 # INFO
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 等待切换完成
|
||
time.sleep(1.0)
|
||
|
||
# 获取当前角度
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
info(f"主要转向完成: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度")
|
||
|
||
# 计算剩余误差
|
||
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 and not target_reached:
|
||
warning(f"剩余误差: {math.degrees(remaining_dist):.2f}°, 需要进行微调", "角度")
|
||
|
||
# 进行微调
|
||
|
||
# 精确模式下使用更小的微调速度
|
||
fine_turn_speed = 0.3 if precision else 0.5
|
||
|
||
# 如果剩余误差很小，进一步降低速度
|
||
if abs(remaining_dist) < 0.1: # 约5.7度
|
||
fine_turn_speed = 0.15
|
||
|
||
# 设置微调命令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [0, 0, fine_turn_speed if remaining_dist > 0 else -fine_turn_speed]
|
||
msg.duration = 0 # 不使用固定时间，而是通过监控角度来停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 速度={fine_turn_speed}", "旋转")
|
||
|
||
# 监控微调过程中的角度变化
|
||
start_time = time.time()
|
||
last_yaw = current_yaw
|
||
stable_count = 0
|
||
timeout = 5 # 微调超时时间
|
||
target_reached = False
|
||
|
||
# 实时监控微调
|
||
while time.time() - start_time < timeout:
|
||
time.sleep(0.1)
|
||
|
||
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
|
||
|
||
# 如果误差在允许范围内，准备停止旋转
|
||
if abs(current_error) <= limit:
|
||
target_reached = True
|
||
debug(f"微调中达到目标角度: 当前={math.degrees(current_yaw_now):.2f}°, 目标={math.degrees(target_yaw):.2f}°", "旋转")
|
||
break
|
||
|
||
# 计算已旋转角度
|
||
rotated = abs(current_yaw_now - last_yaw)
|
||
if rotated > math.pi:
|
||
rotated = 2 * math.pi - rotated
|
||
|
||
# 如果旋转速度很小（几乎停止）
|
||
if rotated < 0.005: # 更小的阈值
|
||
stable_count += 1
|
||
else:
|
||
stable_count = 0
|
||
|
||
last_yaw = current_yaw_now
|
||
|
||
# 如果机器人稳定一段时间，认为旋转完成
|
||
if stable_count >= 5:
|
||
debug(f"微调已稳定，准备停止", "旋转")
|
||
break
|
||
|
||
# 切换到站立姿态
|
||
debug("微调停止，切换到站立姿态", "姿态")
|
||
msg.mode = 1 # 切换到站立模式
|
||
msg.gait_id = 0
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.duration = 0
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 等待切换完成
|
||
time.sleep(1.0)
|
||
|
||
# 获取最终角度
|
||
final_yaw = ctrl.odo_msg.rpy[2]
|
||
info(f"微调完成: 最终角度={math.degrees(final_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度")
|
||
|
||
final_error = abs(target_yaw - final_yaw)
|
||
if final_error > math.pi:
|
||
final_error = 2 * math.pi - final_error
|
||
|
||
# 判断是否成功达到目标
|
||
if final_error <= limit:
|
||
success(f"旋转成功，误差在允许范围内: {math.degrees(final_error):.2f}°", "成功")
|
||
return True
|
||
else:
|
||
warning(f"旋转完成，但误差超出允许范围: {math.degrees(final_error):.2f}° > {math.degrees(limit):.2f}°", "警告")
|
||
|
||
# 在精确模式下，如果误差仍然很大，尝试第二次微调
|
||
if precision and final_error > limit * 2:
|
||
info("尝试第二次微调", "精确")
|
||
|
||
# 计算第二次微调角度
|
||
second_adj = target_yaw - final_yaw
|
||
if second_adj > math.pi:
|
||
second_adj -= 2 * math.pi
|
||
elif second_adj < -math.pi:
|
||
second_adj += 2 * math.pi
|
||
|
||
# 降低微调速度
|
||
very_fine_speed = 0.1
|
||
|
||
# 设置第二次微调命令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [0, 0, very_fine_speed if second_adj > 0 else -very_fine_speed]
|
||
msg.duration = 0 # 不使用固定时间
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
debug(f"第二次微调：旋转{math.degrees(second_adj):.2f}°", "精确")
|
||
|
||
# 使用实时监控等待第二次微调完成
|
||
start_time = time.time()
|
||
last_yaw = final_yaw
|
||
stable_count = 0
|
||
timeout = 5
|
||
target_reached = False
|
||
|
||
# 实时监控第二次微调
|
||
while time.time() - start_time < timeout:
|
||
time.sleep(0.1)
|
||
|
||
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
|
||
|
||
# 如果误差在允许范围内，准备停止旋转
|
||
if abs(current_error) <= limit:
|
||
target_reached = True
|
||
debug(f"第二次微调达到目标角度: 当前={math.degrees(current_yaw_now):.2f}°, 目标={math.degrees(target_yaw):.2f}°", "旋转")
|
||
break
|
||
|
||
# 计算已旋转角度
|
||
rotated = abs(current_yaw_now - last_yaw)
|
||
if rotated > math.pi:
|
||
rotated = 2 * math.pi - rotated
|
||
|
||
# 如果旋转速度很小（几乎停止）
|
||
if rotated < 0.005: # 更小的阈值
|
||
stable_count += 1
|
||
else:
|
||
stable_count = 0
|
||
|
||
last_yaw = current_yaw_now
|
||
|
||
# 如果机器人稳定一段时间，认为旋转完成
|
||
if stable_count >= 5:
|
||
debug(f"第二次微调已稳定，准备停止", "旋转")
|
||
break
|
||
|
||
# 切换到站立姿态
|
||
debug("第二次微调停止，切换到站立姿态", "姿态")
|
||
msg.mode = 1 # 切换到站立模式
|
||
msg.gait_id = 0
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.duration = 0
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 等待切换完成
|
||
time.sleep(1.0)
|
||
|
||
# 获取最终结果
|
||
final_final_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
final_final_error = abs(target_yaw - final_final_yaw)
|
||
if final_final_error > math.pi:
|
||
final_final_error = 2 * math.pi - final_final_error
|
||
|
||
if final_final_error <= limit:
|
||
success(f"第二次微调成功，最终误差: {math.degrees(final_final_error):.2f}°", "成功")
|
||
return True
|
||
else:
|
||
warning(f"两次微调后仍有误差: {math.degrees(final_final_error):.2f}°", "警告")
|
||
# 如果误差已经比第一次小，算作改进
|
||
return final_final_error < final_error
|
||
|
||
return False
|
||
|
||
# 如果直接达到目标，返回成功
|
||
if target_reached:
|
||
success(f"旋转直接达到目标，误差在允许范围内", "成功")
|
||
return True
|
||
|
||
success("旋转成功完成", "完成")
|
||
return True
|
||
|
||
def turn_degree_twice_2(ctrl, msg, degree=90, absolute=False, precision=True):
|
||
"""
|
||
使用turn_degree_2函数进行两次旋转尝试
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
|
||
absolute: 是否绝对角度，默认为 False
|
||
precision: 是否启用高精度模式，该模式下会使用更慢的速度和更细致的微调，默认为 False
|
||
|
||
返回:
|
||
Bool: 是否成功旋转到指定角度
|
||
"""
|
||
aligned = turn_degree_2(ctrl, msg, degree=degree, absolute=absolute, precision=precision)
|
||
if not aligned:
|
||
turn_degree_2(ctrl, msg, degree=degree, absolute=absolute, precision=precision)
|
||
return aligned
|
||
|