105 lines
3.9 KiB
Python
105 lines
3.9 KiB
Python
|
import time
|
|||
|
|
import sys
|
|||
|
|
import os
|
|||
|
|
import toml
|
|||
|
|
|
|||
|
|
# 添加父目录到路径，以便能够导入utils
|
|||
|
|
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
|||
|
|
# 添加当前目录到路径，确保可以找到local文件
|
|||
|
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
|||
|
|
|
|||
|
|
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
|||
|
|
from base_move.turn_degree import turn_degree
|
|||
|
|
from base_move.go_straight import go_straight
|
|||
|
|
from file_send_lcmt import file_send_lcmt
|
|||
|
|
|
|||
|
|
# 创建本模块特定的日志记录器
|
|||
|
|
logger = get_logger("任务3")
|
|||
|
|
|
|||
|
|
observe = True
|
|||
|
|
|
|||
|
|
def load_gait_file(file_path):
|
|||
|
|
"""加载步态文件"""
|
|||
|
|
try:
|
|||
|
|
with open(file_path, 'r') as f:
|
|||
|
|
return f.read()
|
|||
|
|
except Exception as e:
|
|||
|
|
error(f"加载步态文件失败: {e}", "文件")
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
def run_task_3(ctrl, msg):
|
|||
|
|
section('任务3：步态切换', "启动")
|
|||
|
|
info('开始执行任务3...', "启动")
|
|||
|
|
|
|||
|
|
# 创建一个LCM通信实例用于发送文件
|
|||
|
|
lc_file = ctrl.lc_s # 复用已有的LCM发送通道
|
|||
|
|
|
|||
|
|
section('任务3-1：切换到直立步态', "步态")
|
|||
|
|
# 加载直立步态文件
|
|||
|
|
gait_up_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_up.toml"))
|
|||
|
|
gait_up_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_up.toml"))
|
|||
|
|
|
|||
|
|
if gait_up_def and gait_up_params:
|
|||
|
|
# 发送步态定义文件
|
|||
|
|
file_msg = file_send_lcmt()
|
|||
|
|
file_msg.data = gait_up_def
|
|||
|
|
lc_file.publish("Gait_Def", file_msg.encode())
|
|||
|
|
time.sleep(0.5) # 等待处理
|
|||
|
|
|
|||
|
|
# 发送步态参数文件
|
|||
|
|
file_msg.data = gait_up_params
|
|||
|
|
lc_file.publish("Gait_Params", file_msg.encode())
|
|||
|
|
time.sleep(0.5) # 等待处理
|
|||
|
|
|
|||
|
|
# 执行步态切换
|
|||
|
|
msg.mode = 62 # 用户自定义步态模式
|
|||
|
|
msg.gait_id = 110 # 根据Usergait_List.toml定义
|
|||
|
|
ctrl.Send_cmd(msg)
|
|||
|
|
|
|||
|
|
# 等待步态切换完成
|
|||
|
|
success("直立步态切换完成", "步态")
|
|||
|
|
time.sleep(2) # 给机器人一些时间适应新步态
|
|||
|
|
|
|||
|
|
# 使用新步态进行简单移动
|
|||
|
|
go_straight(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
|
|||
|
|
else:
|
|||
|
|
error("直立步态文件加载失败，无法执行任务3-1", "错误")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
section('任务3-2：切换到后退步态', "步态")
|
|||
|
|
# 加载后退步态文件
|
|||
|
|
gait_moonwalk_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_moonwalk.toml"))
|
|||
|
|
gait_moonwalk_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_moonwalk.toml"))
|
|||
|
|
|
|||
|
|
if gait_moonwalk_def and gait_moonwalk_params:
|
|||
|
|
# 发送步态定义文件
|
|||
|
|
file_msg = file_send_lcmt()
|
|||
|
|
file_msg.data = gait_moonwalk_def
|
|||
|
|
lc_file.publish("Gait_Def", file_msg.encode())
|
|||
|
|
time.sleep(0.5) # 等待处理
|
|||
|
|
|
|||
|
|
# 发送步态参数文件
|
|||
|
|
file_msg.data = gait_moonwalk_params
|
|||
|
|
lc_file.publish("Gait_Params", file_msg.encode())
|
|||
|
|
time.sleep(0.5) # 等待处理
|
|||
|
|
|
|||
|
|
# 执行步态切换
|
|||
|
|
msg.mode = 62 # 用户自定义步态模式
|
|||
|
|
msg.gait_id = 110 # 根据Usergait_List.toml定义
|
|||
|
|
ctrl.Send_cmd(msg)
|
|||
|
|
|
|||
|
|
# 等待步态切换完成
|
|||
|
|
success("后退步态切换完成", "步态")
|
|||
|
|
time.sleep(2) # 给机器人一些时间适应新步态
|
|||
|
|
|
|||
|
|
# 使用新步态进行简单移动
|
|||
|
|
go_straight(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
|
|||
|
|
else:
|
|||
|
|
error("后退步态文件加载失败，无法执行任务3-2", "错误")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 回到默认步态
|
|||
|
|
section('任务3-3：恢复默认步态', "步态")
|
|||
|
|
ctrl.base_msg.stand_up() # 使用基础消息模块恢复到默认站立姿态
|
|||
|
|
|
|||
|
|
success("任务3完成", "完成")
|