Enhance task 2 functionality and add backtracking capability

- Introduced run_task_2_back function to facilitate reverse navigation.
- Updated main.py to include calls to run_task_2_back alongside run_task_2 for improved task execution.
- Adjusted movement parameters in run_task_2 for better path accuracy and control.
- Added new movement functions for enhanced maneuverability in task 2.

main.py

@@ -21,12 +21,14 @@ from utils.image_raw import ImageProcessor
 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.task_2 import run_task_2, run_task_2_back
 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
 # from task_test.go_to_xy_example import run_go_to_xy_example
 
+from base_move.turn_degree import turn_degree
+
 pass_marker = True
 
 def main():
@@ -43,11 +45,12 @@ def main():
         # time.sleep(100) # TEST 
         # run_task_1(Ctrl, msg)
 
-        # run_task_2(Ctrl, msg)
+        run_task_2(Ctrl, msg)
-
+        turn_degree(Ctrl, msg, -90, absolute=True)
+        run_task_2_back(Ctrl, msg)
         # run_task_2_5(Ctrl, msg)
 
-        run_task_4(Ctrl, msg)
+        # run_task_4(Ctrl, msg)
 
         # run_task_test(Ctrl, msg)
         

task_2/task_2.py

@@ -8,6 +8,13 @@ from base_move.go_to_xy import go_to_xy
 from base_move.turn_degree import turn_degree
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 
+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
+)
+
 # 创建本模块特定的日志记录器
 logger = get_logger("任务1")
 
@@ -49,25 +56,31 @@ def run_task_2(ctrl, msg):
     # msg.life_count += 1
     # Ctrl.Send_cmd(msg)
     # time.sleep(1)
-
+    print('角度为',ctrl.odo_msg.rpy[2])
     #获取微调角度
-
+    text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
     # 连续三重弯道行走
     directions = [
         #直线走一段
-        (0.4,0,0,0,0,0,1.6),
-        #第一个圆弧
-        (0.45, 0, 0.77,0,0,0,7),
-        #微调路径
         (0.4,0,0,0,0,0,1.2),
-        # #第二个圆弧
+        #第一个圆弧
-        (0.4,0,-0.71,0,0,0,7.7),
+        (0.45, 0, 0.77,0,0,0,6.5),
-        # #第三个圆弧
+        #微调路径
-        (0.4,0.05,-0.75,0,0,0,1.2),
+        (0.3,0,0,0,0,0,0.4),
-        # #微调路径
+        #第二个圆弧
-        (0.45, 0, 0.8,0,0,0,7.2),
+        (0.4,0,-0.73,0,0,0,7.9),
-        # #第四个圆弧
+        #微调路径
-        (0.4,0,-0.71,0,0,0,4)
+        (0.3,0,0,0,0,0,1),
+        # 第三个圆弧
+        (0.4,0,0.73,0,0,0,6.4),
+        #微调路径
+        (0.3,0,0,0,0,0,1.3),
+        #第四个圆弧
+        (0.4,0,-0.69,0,0,0,3.6),
+        #绕出来
+        (0.3,0,0.6,0,0,0,1),
+        # #前走一点
+        # (0.3,0,0,0,0,0,0.5)
         ]  # [vel_x, vel_z] 对应 [左转, 右转, 左转]
     
 
@@ -83,14 +96,61 @@ def run_task_2(ctrl, msg):
         ctrl.Send_cmd(msg)
         
         # 打印当前方向
-        print(f"开始第 {i+1} 次转弯")
+        print(f"开始 {text[i]} ")
         
         # 根据转弯方向调整持续时间（假设半圆需要3秒）
         time.sleep(t) # 持续3秒
 
+    print('角度为',ctrl.odo_msg.rpy[2])
+    print('x为',ctrl.odo_msg.xyz[0])
+    print('y为',ctrl.odo_msg.xyz[1])
+    print('z为',ctrl.odo_msg.xyz[2])
     # # 停止
     # msg.mode = 7    # PureDamper
     # msg.gait_id = 0
     # msg.life_count += 1
     # Ctrl.Send_cmd(msg)
     # Ctrl.Wait_finish(7, 0)
+
+def run_task_2_back(ctrl, msg):
+
+    text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
+    
+    # 连续三重弯道行走
+    directions = [
+        (0.3,0,0.6,0,0,0,1),
+        #第四个圆弧
+        (0.4,0,0.69,0,0,0,3.6),
+        #微调路径
+        (0.3,0,0,0,0,0,1.3),
+        # 第三个圆弧
+        (0.4,0,-0.73,0,0,0,6.4),
+        #微调路径
+        (0.3,0,0,0,0,0,1),
+        #第二个圆弧
+        (0.4,0,0.73,0,0,0,7.9),
+        #微调路径
+        (0.3,0,0,0,0,0,0.4),
+        #第一个圆弧
+        (0.45, 0, -0.77,0,0,0,6.5),
+        #直线走一段
+        (0.4,0,0,0,0,0,1.2),
+    ]  # [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"开始 {text[i]} ")
+        
+        # 根据转弯方向调整持续时间（假设半圆需要3秒）
+        time.sleep(t) # 持续3秒