import math import time import sys import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from utils.localization_lcmt import localization_lcmt def go_straight(ctrl, msg, distance, speed=0.5, observe=False): """ 控制机器人沿直线行走指定距离 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 distance: 要行走的距离(米)，正值为前进，负值为后退 speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5 observe: 是否输出中间状态信息，默认为False 返回: bool: 是否成功完成行走 """ # 参数验证 if abs(distance) < 0.01: print("距离太短，无需移动") return True # 限制速度范围 speed = min(max(abs(speed), 0.1), 1.0) # 确定前进或后退方向 forward = distance > 0 move_speed = speed if forward else -speed abs_distance = abs(distance) # 获取起始位置 start_position = list(ctrl.odo_msg.xyz) start_yaw = ctrl.odo_msg.rpy[2] # 记录起始朝向，用于保持直线 if observe: print(f"起始位置: {start_position}") print(f"开始{'前进' if forward else '后退'} {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒") # 在起点放置标记 if hasattr(ctrl, 'place_marker'): ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) # 设置移动命令 msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 # 根据需要移动的距离动态调整移动速度 if abs_distance > 1.0: actual_speed = move_speed # 距离较远时用设定速度 elif abs_distance > 0.5: actual_speed = move_speed * 0.8 # 中等距离略微降速 elif abs_distance > 0.2: actual_speed = move_speed * 0.6 # 较近距离降低速度 else: actual_speed = move_speed * 0.4 # 非常接近时用更慢速度 # 设置移动速度和方向 msg.vel_des = [actual_speed, 0, 0] # [前进速度, 侧向速度, 角速度] msg.duration = 0 # wait next cmd msg.step_height = [0.06, 0.06] # 抬腿高度 msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) # 估算移动时间，但实际上会通过里程计控制 estimated_time = abs_distance / abs(actual_speed) timeout = estimated_time + 3 # 增加超时时间为预计移动时间加3秒 # 使用里程计进行实时监控移动距离 distance_moved = 0 start_time = time.time() last_position = start_position # 动态调整参数 angle_correction_threshold = 0.05 # 角度偏差超过多少弧度开始修正 slow_down_ratio = 0.85 # 当移动到目标距离的85%时开始减速 completion_threshold = 0.95 # 当移动到目标距离的95%时停止 position_check_interval = 0.1 # 位置检查间隔(秒) last_check_time = start_time # 监控移动距离 while distance_moved < abs_distance * completion_threshold and time.time() - start_time < timeout: current_time = time.time() # 按固定间隔检查位置，减少计算负担 if current_time - last_check_time >= position_check_interval: # 获取当前位置和朝向 current_position = ctrl.odo_msg.xyz current_yaw = ctrl.odo_msg.rpy[2] # 计算已移动距离 dx = current_position[0] - start_position[0] dy = current_position[1] - start_position[1] distance_moved = math.sqrt(dx*dx + dy*dy) # 计算移动方向与初始朝向的偏差 movement_direction = math.atan2(current_position[1] - last_position[1], current_position[0] - last_position[0]) if distance_moved > 0.05 else start_yaw yaw_error = movement_direction - start_yaw # 角度归一化 while yaw_error > math.pi: yaw_error -= 2 * math.pi while yaw_error < -math.pi: yaw_error += 2 * math.pi # 计算完成比例 completion_ratio = distance_moved / abs_distance # 根据距离完成情况调整速度 if completion_ratio > slow_down_ratio: # 计算减速系数 slow_factor = 1.0 - (completion_ratio - slow_down_ratio) / (1.0 - slow_down_ratio) # 确保不会减速太多 slow_factor = max(0.2, slow_factor) new_speed = actual_speed * slow_factor if observe and abs(new_speed - msg.vel_des[0]) > 0.05: print(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)") msg.vel_des[0] = new_speed msg.life_count += 1 ctrl.Send_cmd(msg) # 如果偏离初始方向，进行角度修正 if abs(yaw_error) > angle_correction_threshold: # 计算角速度修正值，偏差越大修正越强 angular_correction = -yaw_error * 0.5 # 比例系数可调整 # 限制最大角速度修正 angular_correction = max(min(angular_correction, 0.2), -0.2) if observe and abs(angular_correction) > 0.05: print(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s") # 应用角速度修正 msg.vel_des[2] = angular_correction msg.life_count += 1 ctrl.Send_cmd(msg) elif msg.vel_des[2] != 0: # 如果方向已修正，重置角速度 msg.vel_des[2] = 0 msg.life_count += 1 ctrl.Send_cmd(msg) if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval): print(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)") print(f"当前速度: [{msg.vel_des[0]:.2f}, {msg.vel_des[1]:.2f}, {msg.vel_des[2]:.2f}]") # 更新最后检查时间和位置 last_check_time = current_time last_position = current_position time.sleep(0.01) # 小间隔检查位置 # 平滑停止 if hasattr(ctrl.base_msg, 'stop_smooth'): ctrl.base_msg.stop_smooth() else: ctrl.base_msg.stop() # 获取最终位置和实际移动距离 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] actual_distance = math.sqrt(dx*dx + dy*dy) if observe: print(f"移动完成，从里程计计算的移动距离: {actual_distance:.3f}米") # 在终点放置标记 if hasattr(ctrl, 'place_marker'): ctrl.place_marker(final_position[0], final_position[1], final_position[2] if len(final_position) > 2 else 0.0, 'red', observe=True) # 判断是否成功完成 distance_error = abs(actual_distance - abs_distance) success = distance_error < 0.1 # 如果误差小于10厘米，则认为成功 if observe: print(f"目标距离: {abs_distance:.3f}米, 实际距离: {actual_distance:.3f}米, 误差: {distance_error:.3f}米") print(f"移动{'成功' if success else '失败'}") return success # 用法示例 if __name__ == "__main__": # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象 # 前进1米 # go_straight(ctrl, msg, 1.0, speed=0.5, observe=True) # 后退0.5米 # go_straight(ctrl, msg, -0.5, speed=0.3, observe=True) pass