From 952524dc63493efb45c11110418237a161668247 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sun, 25 May 2025 18:31:29 +0000 Subject: [PATCH] refactor(task_3): remove unnecessary code and update functionality - Remove redundant LCM initialization and file writing in main.py - Update file paths in task_3.py to use relative paths - Replace ctrl.Send_msg() with ctrl.Send_cmd(msg) for consistency - Add import lcm in task_3.py - Update main.py to focus on user gait list processing --- task_3/main.py | 45 --------------------------------------------- task_3/task_3.py | 22 +++++++++++++--------- 2 files changed, 13 insertions(+), 54 deletions(-) diff --git a/task_3/main.py b/task_3/main.py index fc86e04..16e0d4c 100755 --- a/task_3/main.py +++ b/task_3/main.py @@ -29,53 +29,8 @@ robot_cmd = { def main(): lcm_cmd = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") - lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") - usergait_msg = file_send_lcmt() cmd_msg = robot_control_cmd_lcmt() try: - steps = toml.load("Gait_Params_up.toml") - full_steps = {'step':[robot_cmd]} - k =0 - for i in steps['step']: - cmd = copy.deepcopy(robot_cmd) - cmd['duration'] = i['duration'] - if i['type'] == 'usergait': - cmd['mode'] = 11 # LOCOMOTION - cmd['gait_id'] = 110 # USERGAIT - cmd['vel_des'] = i['body_vel_des'] - cmd['rpy_des'] = i['body_pos_des'][0:3] - cmd['pos_des'] = i['body_pos_des'][3:6] - cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2] - cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5] - cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8] - cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11] - cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3 - cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3 - cmd['acc_des'] = i['weight'] - cmd['value'] = i['use_mpc_traj'] - cmd['contact'] = math.floor(i['landing_gain'] * 1e1) - cmd['ctrl_point'][2] = i['mu'] - if k == 0: - full_steps['step'] = [cmd] - else: - full_steps['step'].append(cmd) - k=k+1 - f = open("Gait_Params_up_full.toml", 'w') - f.write("# Gait Params\n") - f.writelines(toml.dumps(full_steps)) - f.close() - - file_obj_gait_def = open("Gait_Def_up.toml",'r') - file_obj_gait_params = open("Gait_Params_up_full.toml",'r') - usergait_msg.data = file_obj_gait_def.read() - lcm_usergait.publish("user_gait_file",usergait_msg.encode()) - time.sleep(0.5) - usergait_msg.data = file_obj_gait_params.read() - lcm_usergait.publish("user_gait_file",usergait_msg.encode()) - time.sleep(0.1) - file_obj_gait_def.close() - file_obj_gait_params.close() - user_gait_list = open("Usergait_List.toml",'r') steps = toml.load(user_gait_list) for step in steps['step']: diff --git a/task_3/task_3.py b/task_3/task_3.py index a531748..cc680f1 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -4,6 +4,7 @@ import os import toml import copy import math +import lcm # 添加父目录到路径，以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) @@ -43,11 +44,14 @@ def load_gait_file(file_path): def run_task_3(ctrl, msg): section('任务3：步态切换', "启动") info('开始执行任务3...', "启动") + + # turn_degree(ctrl, msg, 90, absolute=True) + usergait_msg = file_send_lcmt() - lcm_usergait = ctrl.lc_s + lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") try: - steps = toml.load("Gait_Params_up.toml") + steps = toml.load("./task_3/Gait_Params_up.toml") full_steps = {'step':[robot_cmd]} k =0 for i in steps['step']: @@ -74,13 +78,13 @@ def run_task_3(ctrl, msg): else: full_steps['step'].append(cmd) k=k+1 - f = open("Gait_Params_up_full.toml", 'w') + f = open("./task_3/Gait_Params_up_full.toml", 'w') f.write("# Gait Params\n") f.writelines(toml.dumps(full_steps)) f.close() - file_obj_gait_def = open("Gait_Def_up.toml",'r') - file_obj_gait_params = open("Gait_Params_up_full.toml",'r') + file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r') + file_obj_gait_params = open("./task_3/Gait_Params_up_full.toml",'r') usergait_msg.data = file_obj_gait_def.read() lcm_usergait.publish("user_gait_file",usergait_msg.encode()) time.sleep(0.5) @@ -90,7 +94,7 @@ def run_task_3(ctrl, msg): file_obj_gait_def.close() file_obj_gait_params.close() - user_gait_list = open("Usergait_List.toml",'r') + user_gait_list = open("./task_3/Usergait_List.toml",'r') steps = toml.load(user_gait_list) for step in steps['step']: msg.mode = step['mode'] @@ -109,15 +113,15 @@ def run_task_3(ctrl, msg): msg.ctrl_point[i] = step['ctrl_point'][i] for i in range(2): msg.step_height[i] = step['step_height'][i] - ctrl.Send_msg() + ctrl.Send_cmd(msg) time.sleep( 0.1 ) for i in range(325): #15s Heat beat It is used to maintain the heartbeat when life count is not updated - ctrl.Send_msg() + ctrl.Send_cmd(msg) time.sleep( 0.2 ) except KeyboardInterrupt: msg.mode = 7 #PureDamper before KeyboardInterrupt: msg.gait_id = 0 msg.duration = 0 msg.life_count += 1 - ctrl.Send_msg() + ctrl.Send_cmd(msg) pass \ No newline at end of file