59 lines
2.4 KiB
Python
59 lines
2.4 KiB
Python
import sys
|
||
import time
|
||
import threading
|
||
|
||
# 原代码5. 运动控制模块完整逻辑（保留原路径导入）
|
||
PROJECT_ROOT = "/home/duckpi/open_duck_mini_ws/OPEN_DUCK_MINI/Open_Duck_Mini_Runtime-2"
|
||
sys.path.append(PROJECT_ROOT)
|
||
from v2_rl_walk_mujoco import RLWalk
|
||
|
||
class RobotMotionController:
|
||
def __init__(self, onnx_model_path, tts_controller, feedback_text):
|
||
# 接收调度脚本传入的TTS实例和反馈文本，保持原逻辑调用
|
||
self.tts_controller = tts_controller
|
||
self.FEEDBACK_TEXT = feedback_text
|
||
try:
|
||
self.rl_walk = RLWalk(
|
||
onnx_model_path=onnx_model_path,
|
||
cutoff_frequency=40,
|
||
pid=[30, 0, 0]
|
||
)
|
||
self.walk_thread = threading.Thread(target=self.rl_walk.run, daemon=True)
|
||
self.walk_thread.start()
|
||
time.sleep(1)
|
||
print("🤖 运动控制模块初始化成功")
|
||
except Exception as e:
|
||
print(f"❌ 运动控制失败：{str(e)}")
|
||
sys.exit(1)
|
||
|
||
def execute_motion(self, action_name: str, seconds: int):
|
||
# 全局变量由调度脚本传入，保留原逻辑
|
||
global is_processing
|
||
is_processing = True
|
||
try:
|
||
# 播放反馈（同步执行确保声音输出）
|
||
self.tts_controller.speak(self.FEEDBACK_TEXT[action_name])
|
||
|
||
# 执行运动
|
||
seconds = max(2, min(seconds, 5))
|
||
if action_name == "move_forward":
|
||
print(f"\n🚶 前进{seconds}秒...")
|
||
self.rl_walk.last_commands[0] = 0.17
|
||
elif action_name == "move_backward":
|
||
print(f"\n🚶 后退{seconds}秒...")
|
||
self.rl_walk.last_commands[0] = -0.17
|
||
elif action_name == "turn_left":
|
||
print(f"\n🔄 左转{seconds}秒...")
|
||
self.rl_walk.last_commands[2] = 1.1
|
||
elif action_name == "turn_right":
|
||
print(f"\n🔄 右转{seconds}秒...")
|
||
self.rl_walk.last_commands[2] = -1.1
|
||
|
||
time.sleep(seconds)
|
||
self.rl_walk.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
print(f"✅ 运动完成")
|
||
except Exception as e:
|
||
print(f"❌ 运动执行失败：{str(e)}")
|
||
self.rl_walk.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
finally:
|
||
is_processing = False |