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

@@ -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']:

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_def = open("./task_3/Gait_Def_up.toml",'r')
-        file_obj_gait_params = open("Gait_Params_up_full.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