Merge branch 'task-2' into main

main.py

@@ -21,6 +21,7 @@ from utils.marker_client import MarkerRunner
 from utils.base_msg import BaseMsg
 
 from task_1.task_1 import run_task_1
+from task_2.task_2 import run_task_2
 from task_2_5.task_2_5 import run_task_2_5
 from task_4.task_4 import run_task_4
 from task_test.task_left_line import run_task_test
@@ -42,6 +43,8 @@ def main():
         # time.sleep(100) # TEST 
         # run_task_1(Ctrl, msg)
 
+        run_task_2(Ctrl, msg)
+
         # run_task_2_5(Ctrl, msg)
 
         # run_task_4(Ctrl, msg)

task_2/task_2.py

@@ -0,0 +1,97 @@
+import time
+import sys
+import os
+import math
+
+# sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+# from base_move.move_base_hori_line import (
+#     arc_turn_around_hori_line,
+#     go_straight_by_hori_line,
+#     move_to_hori_line,
+#     align_to_horizontal_line
+# )
+# from base_move.go_straight import go_straight
+# from base_move.turn_degree import turn_degree
+from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
+
+# 创建本模块特定的日志记录器
+logger = get_logger("任务1")
+
+observe = True
+
+def run_task_2(ctrl, msg):
+
+    # print("Recovery stand")
+    # Ctrl.base_msg.stand_up()
+    
+    # # time.sleep(100) # TEST 
+
+    # run_task_1(Ctrl, msg)
+
+    # x,y=image_processor.get_current_x()
+    # print('分别是',x,y)
+    # # 初始恢复站立
+    # msg.mode = 12 # Recovery stand
+    # msg.gait_id = 0
+    # msg.life_count += 1
+    # Ctrl.Send_cmd(msg)
+    # Ctrl.Wait_finish(12, 0)
+    # print('1')
+    # x,y=image_processor.get_current_x()
+    # print('分别是',x,y)
+
+    # # 初始恢复站立
+    # msg.mode = 11 # Recovery stand
+    # msg.gait_id = 27
+    # msg.vel_des = [0.3,0,0]
+    # msg.pos_des[2]=0.24
+    # msg.duration = 0
+    # msg.step_height = [0.01, 0.01]
+    # msg.life_count += 1
+    # Ctrl.Send_cmd(msg)
+    # time.sleep(1)
+
+    #获取微调角度
+    
+
+    # 连续三重弯道行走
+    directions = [
+        #直线走一段
+        (0.4,0,0,0,0,0,0.5),
+        #第一个圆弧
+        (0.45, 0, 0.77,0,0,0,7.3),
+        #微调路径
+        (0.4,0,0,0,0,0,0.8),
+        #第二个圆弧
+        (0.4,0,-0.72,0,0,0,7.7),
+        #第三个圆弧
+        (0.4,0.05,-0.75,0,0,0,1.2),
+        #微调路径
+        (0.45, 0, 0.8,0,0,0,7.2),
+        #第四个圆弧
+        (0.4,0,-0.72,0,0,0,4)]  # [vel_x, vel_z] 对应 [左转, 右转, 左转]
+    
+
+    for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions):
+        # 设置转向命令
+        msg.mode = 11 # Locomotion
+        msg.gait_id = 26 # TROT_FAST
+        msg.vel_des = [vel_x, vel_y, vel_z] # [x, y, z]，y=0表示不侧移
+        msg.rpy_des = [rpy_x, rpy_y, rpy_z]
+        msg.duration = 0 # 连续运动
+        msg.step_height = [0.03, 0.03]
+        msg.life_count += 1
+        ctrl.Send_cmd(msg)
+        
+        # 打印当前方向
+        print(f"开始第 {i+1} 次转弯")
+        
+        # 根据转弯方向调整持续时间（假设半圆需要3秒）
+        time.sleep(t) # 持续3秒
+
+    # # 停止
+    # msg.mode = 7    # PureDamper
+    # msg.gait_id = 0
+    # msg.life_count += 1
+    # Ctrl.Send_cmd(msg)
+    # Ctrl.Wait_finish(7, 0)