Refactor main.py and task_4: Updated turning logic in main.py to use turn_degree_v2 with a fixed degree of -90. Adjusted Gait_Params_stoop_full.toml and Gait_Params_stoop.toml to reduce body velocity from 0.2 to 0.15 for smoother movement. Enhanced pass_bar.py to streamline gait file publishing and added step_num variable for better control during the pass_bar function. Updated run_task_4_back to improve task flow and added a pause for standing up after passing the bar.

main.py

@@ -56,6 +56,7 @@ def main():
         # print(f"arrow_direction: {arrow_direction}")
         # run_task_2_5(Ctrl, msg, direction=arrow_direction)
 
+        turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
         arrow_direction = 'right' # TEST
 
         # if arrow_direction == 'left':
@@ -63,11 +64,9 @@ def main():
         # else:
         #     run_task_3(Ctrl, msg)
 
-        turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
+        # turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
 
-        run_task_5(Ctrl, msg, direction=arrow_direction)
+        # run_task_5(Ctrl, msg, direction=arrow_direction)
-
-        return
 
         if arrow_direction == 'left':
             # run_task_3_back(Ctrl, msg)
@@ -75,6 +74,8 @@ def main():
         else:
             run_task_4_back(Ctrl, msg)
 
+        return
+
         run_task_5(Ctrl, msg)
 
         run_task_2_5_back(Ctrl, msg, direction=arrow_direction)

task_4/pass_bar.py

@@ -37,9 +37,11 @@ def pass_bar(ctrl, msg):
     """
     俯身通过一个栅栏
     """
+    step_num = 8
+
     # 切换步态
     usergait_msg = file_send_lcmt()
-    lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
+    # lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
     
     try:
         steps = toml.load("./task_4/Gait_Params_stoop.toml")
@@ -78,20 +80,14 @@ def pass_bar(ctrl, msg):
         file_obj_gait_def = open("./task_4/Gait_Def_stoop.toml",'r')
         file_obj_gait_params = open("./task_4/Gait_Params_stoop_full.toml",'r')
         usergait_msg.data = file_obj_gait_def.read()
-        lcm_usergait.publish("user_gait_file",usergait_msg.encode())
+        ctrl.lc_s.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())
+        ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
         time.sleep(0.1)
         file_obj_gait_def.close()
         file_obj_gait_params.close()
 
-        file_obj_gait_params = open("./task_4/Gait_Params_stoop_full.toml",'r')
-        usergait_msg.data = file_obj_gait_params.read()
-        lcm_usergait.publish("user_gait_file", usergait_msg.encode())
-        time.sleep(0.5)
-        file_obj_gait_params.close()
-
         msg.mode = 62
         msg.value = 0
         msg.contact = 15
@@ -99,9 +95,9 @@ def pass_bar(ctrl, msg):
         msg.duration = 1000
         msg.life_count += 1
         
-        for i in range(50):
+        for i in range(step_num):
             ctrl.Send_cmd(msg)
-            time.sleep(0.2)
+            time.sleep(1)
 
     except KeyboardInterrupt:
         msg.mode = 7 #PureDamper before KeyboardInterrupt:

task_4/task_4.py

@@ -45,12 +45,18 @@ def run_task_4_back(ctrl, msg):
         msg: 控制消息对象
         image_processor: 可选的图像处理器实例
     """
-    section('任务4-1：移动直到灰色天空比例小于阈值', "天空检测")
+    section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
-    go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)
+    go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.30)
 
     section('任务4-2：通过栏杆', "移动")
     pass_bar(ctrl, msg)
     
+    go_straight(ctrl, msg, distance=0.5, speed=0.35)
+    time.sleep(5)
+    ctrl.base_msg.stand_up()
+
+    return
+
     section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
     # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
     go_straight_until_hori_line(
@@ -66,20 +72,25 @@ def run_task_4_back(ctrl, msg):
     )
 
 
-def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=10, speed=0.3):
+def go_straight_until_sky_ratio_below(ctrl, msg,
+                                      sky_ratio_threshold=0.2,
+                                      step_distance=0.5,
+                                      max_distance=2,
+                                      speed=0.3
+                                      ):
     """
-    控制机器人沿直线行走，直到灰色天空比例小于指定阈值
+    控制机器人沿直线行走，直到灰色天空比例低于指定阈值
     
     参数:
         ctrl: Robot_Ctrl对象
         msg: 控制命令消息对象
-        sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例小于此值时停止
+        sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例低于此值时停止
         step_distance: 每次移动的步长(米)
         max_distance: 最大移动距离(米)，防止无限前进
         speed: 移动速度(米/秒)
     
     返回:
-        bool: 是否成功找到天空比例小于阈值的位置
+        bool: 是否成功找到天空比例低于阈值的位置
     """
     total_distance = 0
     success_flag = False
@@ -106,9 +117,9 @@ def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_d
             sky_ratio = analyze_gray_sky_ratio(temp_image_path)
             info(f"当前灰色天空比例: {sky_ratio:.2%}", "分析")
             
-            # 如果天空比例小于阈值，停止移动
+            # 如果天空比例高于阈值，停止移动
             if sky_ratio < sky_ratio_threshold:
-                success(f"检测到灰色天空比例({sky_ratio:.2%})小于阈值({sky_ratio_threshold:.2%})，停止移动", "完成")
+                success(f"检测到灰色天空比例({sky_ratio:.2%})低于阈值({sky_ratio_threshold:.2%})，停止移动", "完成")
                 success_flag = True
                 break