Remove Gait_Params_up_full.toml file and update task_3.py and task_4.py to enhance navigation and control. Integrate go_lateral function for lateral movement, improve task execution with center_on_dual_tracks, and streamline file publishing for gait parameters. Update detection logic for horizontal lines and adjust iteration thresholds for better performance.

base_move/move_base_hori_line.py

@@ -1179,9 +1179,10 @@ def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0
                 edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
             elif detect_func_version == 3:
                 edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
+            elif detect_func_version == 4:
+                edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
             else:
                 error("未指定检测版本，请检查参数", "失败")
-                edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
             
             if edge_point is not None and edge_info is not None:
                 line_detected = True
@@ -1197,7 +1198,7 @@ def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0
                     info(f"检测到横向线，当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离")
                 
                 # 判断是否达到目标距离
-                if current_distance <= target_distance + 0.1:  # 允许0.1米的误差
+                if current_distance <= target_distance + 0.1:  # additional_distance 额外距离
                     success(f"已达到目标距离，当前距离: {current_distance:.3f}米", "完成")
                     break
                 
@@ -1270,6 +1271,10 @@ def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0
             edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
         elif detect_func_version == 3:
             edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
+        elif detect_func_version == 4:  
+            edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
+        else:
+            error("未指定检测版本，请检查参数", "失败")
         
         if edge_point is not None and edge_info is not None:
             final_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)

task_3/task_3.py

@@ -16,8 +16,9 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)))
 
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 from base_move.turn_degree import turn_degree, turn_degree_v2
-from base_move.go_straight import go_straight
+from base_move.go_straight import go_straight, go_lateral
 from base_move.go_to_xy import go_to_x_v2
+from base_move.center_on_dual_tracks import center_on_dual_tracks
 from file_send_lcmt import file_send_lcmt
 from utils.yellow_area_analyzer import analyze_yellow_area_ratio
 
@@ -41,7 +42,7 @@ robot_cmd = {
 
 def pass_up_down(ctrl, msg):
     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_3/Gait_Params_up.toml")
@@ -80,10 +81,10 @@ def pass_up_down(ctrl, msg):
         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())
+        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()
@@ -100,8 +101,8 @@ def pass_up_down(ctrl, msg):
         stable_threshold = 8  # 连续15次检测z轴不再增加则认为已经停止
         z_speed_threshold = 0.01  # z轴速度阈值，小于这个值认为已经停止爬升
         climb_speed_threshold = 0.05  # 检测到开始爬坡的速度阈值
-        max_iterations = 250  # 最大循环次数，作为安全保障
+        max_iterations = 320  # 最大循环次数，作为安全保障
-        min_iterations = 150  # 最小循环次数，作为安全保障
+        min_iterations = 250  # 最小循环次数，作为安全保障
 
         # 姿态判断参数
         pitch_threshold = 0.05  # 俯仰角阈值（弧度）
@@ -139,7 +140,7 @@ def pass_up_down(ctrl, msg):
             
             # 每10次迭代打印一次当前信息
             if i % 10 == 0:
-                info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
+                info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
             
             # 检测是否开始爬坡阶段
             if not climbing_detected and vz > climb_speed_threshold:
@@ -189,6 +190,8 @@ def pass_up_down(ctrl, msg):
     # msg.vel_des = [0, 0.2, 0]  # [前进速度, 侧向速度, 角速度]
     # msg.life_count += 1
     # ctrl.Send_cmd(msg)
+    turn_degree_v2(ctrl, msg, 90, absolute=True)
+    go_straight(ctrl, msg, distance=0.5, speed=0.5, observe=True)
     time.sleep(1)
     # go_to_x_v2(ctrl, msg, 2, speed=0.5, precision=True, observe=True)
 
@@ -230,10 +233,10 @@ def pass_up_down(ctrl, msg):
         file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r')
         file_obj_gait_params = open("./task_3/Gait_Params_down_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()
@@ -250,8 +253,8 @@ def pass_up_down(ctrl, msg):
         stable_threshold = 10  # 连续10次检测z轴速度接近零则认为已经到达平地
         z_speed_threshold = 0.005  # z轴速度阈值，小于这个值认为已经停止下降
         descent_speed_threshold = -0.05  # 检测到开始下坡的速度阈值（负值表示下降）
-        max_iterations = 250  # 最大循环次数，作为安全保障
+        max_iterations = 120  # 最大循环次数，作为安全保障
-        min_iterations = 150  # 最小循环次数，确保有足够的时间开始动作
+        min_iterations = 80  # 最小循环次数，确保有足够的时间开始动作
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
         
         # 姿态判断参数
@@ -289,7 +292,7 @@ def pass_up_down(ctrl, msg):
             
             # 每10次迭代打印一次当前信息
             if observe and i % 10 == 0:
-                info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
+                info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
             
             # 检测是否开始下坡阶段
             if not descending_detected and vz < descent_speed_threshold:
@@ -495,13 +498,18 @@ def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_
 
 
 def run_task_3(ctrl, msg):
-    section('任务3：步态切换', "启动")
+    section('任务3：上下坡', "启动")
     info('开始执行任务3...', "启动")
 
     turn_degree_v2(ctrl, msg, 90, absolute=True)
+    go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True)
 
-    # section('任务3-1：up', "开始")
+    section('任务3-1：up', "开始")
-    # pass_up_down(ctrl, msg)
+    pass_up_down(ctrl, msg)
+
+    section('任务3-2：center on dual tracks', "开始")
+    turn_degree_v2(ctrl, msg, 90, absolute=True)
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
 
     section('任务3-2：yellow stop', "开始")
     go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)

task_4/task_4.py

@@ -11,10 +11,8 @@ from base_move.turn_degree import turn_degree, turn_degree_v2
 from base_move.go_straight import go_straight, go_lateral
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 from utils.gray_sky_analyzer import analyze_gray_sky_ratio
-from base_move.move_base_hori_line import go_straight_until_hori_line
+from utils.detect_track import detect_horizontal_track_edge
-from base_move.follow_dual_tracks import go_straight_with_visual_track
+from base_move.move_base_hori_line import calculate_distance_to_line
-from base_move.center_on_dual_tracks import center_on_dual_tracks
-
 from task_4.pass_bar import pass_bar
 
 # 创建本模块特定的日志记录器
@@ -52,7 +50,7 @@ def run_task_4_back(ctrl, msg):
 
     # 向右移动0.5秒
     section('任务4-回程：向右移动', "移动")
-    go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True)
+    go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True)  # DEBUG
 
     section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
     go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5)
@@ -62,29 +60,16 @@ def run_task_4_back(ctrl, msg):
 
     section('任务4-3：stone', "移动")
     go_straight(ctrl, msg, distance=1, speed=2)
-    # go_straight_with_visual_track(ctrl, msg, distance=4.5, observe=False,
+
-    #                    mode=11,
-    #                    speed=0.35,
-    #                    gait_id=3,
-    #                    step_height=[0.21, 0.21],
-    #                    )
     go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21])
 
     section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
     # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
-    go_straight_until_hori_line(
+    # 获取相机高度
-        ctrl, 
+    camera_height = 0.355  # 单位: 米 # INFO from TF-tree
-        msg, 
+    edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=True, save_log=True)
-        target_distance=1,   # 目标与黄线的距离(米)
+    current_distance = calculate_distance_to_line(edge_info, camera_height, observe=True)
-        max_distance=6.0,      # 最大搜索距离(米)
+    go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])
-        speed=0.2,            # 移动速度(米/秒)
-        mode=11,               # 运动模式
-        gait_id=3,             # 步态ID（快步跑）
-        step_height=[0.21, 0.21],  # 摆动腿离地高度
-        observe=True           # 显示调试信息
-    )
-
-    go_straight(ctrl, msg, distance=0.8, speed=0.2, observe=True, mode=11, gait_id=3, step_height=[0.21, 0.21])
 
 
 def go_straight_until_sky_ratio_below(ctrl, msg,

task_5/task_5.py

@@ -16,6 +16,7 @@ from base_move.move_base_hori_line import (
     detect_horizontal_track_edge, detect_horizontal_track_edge_v2, detect_horizontal_track_edge_v3, 
     calculate_distance_to_line, move_to_hori_line, arc_turn_around_hori_line
 )
+from base_move.center_on_dual_tracks import center_on_dual_tracks
 # from base_move.follow_dual_tracks import follow_dual_tracks
 
 
@@ -209,6 +210,8 @@ def run_task_5(ctrl, msg, direction='left', observe=False):
     """
     走向卸货
     """
+
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
     section('任务5-1：直线移动并扫描二维码', "移动")
     
     # 最大移动距离为8米
@@ -228,6 +231,7 @@ def run_task_5(ctrl, msg, direction='left', observe=False):
         error("未能成功到达横线前指定距离", "失败")
 
     section('任务5-2：移动到卸货点', "移动")
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
     if direction == 'right' and res['qr_result'] == 'B-2' or direction == 'left' and res['qr_result'] == 'B-1':
         # 直走
         move_to_hori_line(ctrl, msg, target_distance=1.4, observe=observe, detect_func_version=4)