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] # 计算目标朝向 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 >= 3: 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=False): """ 结合里程计实现精确稳定的旋转指定角度 参数: 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)