Merge branch 'task-3-2' of ssh://120.27.199.238:222/Havoc420mac/mi-task into task-3-2

base_move/center_on_dual_tracks.py

@@ -9,7 +9,9 @@ from utils.detect_dual_track_lines import detect_dual_track_lines
 
 def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False,
                          mode=11, gait_id=26, step_height=[0.06, 0.06], 
-                         estimated_track_distance=1.0):
+                         estimated_track_distance=1.0,
+                         detect_height=0.4,
+                         detection_interval=0.2):
     """
     控制机器狗仅使用Y轴移动调整到双轨道线的中间位置
     
@@ -23,6 +25,8 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
     gait_id: 步态ID，默认为26
     step_height: 抬腿高度，默认为[0.06, 0.06]
     estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
+    detect_height: 检测高度，默认为0.4
+    detection_interval: 检测间隔时间(秒)，默认为0.2秒
     
     返回:
     bool: 是否成功调整到中心位置
@@ -37,6 +41,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
     
     # 记录起始时间
     start_time = time.time()
+    last_detection_time = 0  # 记录上次检测时间
     
     # 记录起始位置
     start_position = list(ctrl.odo_msg.xyz)
@@ -104,7 +109,6 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
                 if stable_count < required_stable_count:
                     # 直接根据实际偏差移动
                     msg.vel_des = [0, actual_deviation, 0]
-                    msg.life_count += 1
                     ctrl.Send_cmd(msg)
                 
                 if stable_count >= required_stable_count:
@@ -211,7 +215,8 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
     
     return success
 def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False,
-                                mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0):
+                                mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0,
+                                detection_interval=0.2):
     """
     先居中到双轨道线中间，然后沿轨道线行走指定距离
     
@@ -226,6 +231,7 @@ def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_
     gait_id: 步态ID，默认为26
     step_height: 抬腿高度，默认为[0.06, 0.06]
     estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
+    detection_interval: 检测间隔时间(秒)，默认为0.2秒
     
     返回:
     bool: 是否成功完成居中和轨道跟随
@@ -243,7 +249,8 @@ def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_
         mode=mode, 
         gait_id=gait_id, 
         step_height=step_height,
-        estimated_track_distance=estimated_track_distance
+        estimated_track_distance=estimated_track_distance,
+        detection_interval=detection_interval
     )
     
     if not centering_success:

main.py

@@ -64,8 +64,6 @@ def main():
         else:
             run_task_3(Ctrl, msg)
 
-        return
-
         turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
         run_task_5(Ctrl, msg, direction=arrow_direction)
 
@@ -195,6 +193,7 @@ class Robot_Ctrl(object):
         self.send_lock.acquire()
         self.delay_cnt = 50
         self.cmd_msg = msg
+        self.cmd_msg.life_count %= 127
         self.send_lock.release()
 
     def place_marker(self, x, y, z, color, observe=False):

task_3/task_3.py

@@ -183,17 +183,10 @@ def pass_up_down(ctrl, msg):
         pass
 
     section('任务3-2：x = 2', "开始")
-    # msg.mode = 11  # Locomotion模式 # DEBUG
-    # msg.gait_id = 26  # 自变频步态
-    # msg.duration = 0  # wait next cmd
-    # msg.step_height = [0.06, 0.06]  # 抬腿高度
-    # msg.vel_des = [0, 0.2, 0]  # [前进速度, 侧向速度, 角速度]
-    # msg.life_count += 1
-    # ctrl.Send_cmd(msg)
     turn_degree_v2(ctrl, msg, 90, absolute=True)
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, detect_height=0.3)
     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)
 
     section('任务3-3：down', "完成")
     try:
@@ -253,7 +246,7 @@ def pass_up_down(ctrl, msg):
         stable_threshold = 10  # 连续10次检测z轴速度接近零则认为已经到达平地
         z_speed_threshold = 0.005  # z轴速度阈值，小于这个值认为已经停止下降
         descent_speed_threshold = -0.05  # 检测到开始下坡的速度阈值（负值表示下降）
-        max_iterations = 100  # 最大循环次数，作为安全保障
+        max_iterations = 110  # 最大循环次数，作为安全保障
         min_iterations = 80  # 最小循环次数，确保有足够的时间开始动作
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
         
@@ -501,17 +494,15 @@ def run_task_3(ctrl, msg):
     section('任务3：上下坡', "启动")
     info('开始执行任务3...', "启动")
 
-    # turn_degree_v2(ctrl, msg, 90, absolute=True)
+    turn_degree_v2(ctrl, msg, 90, absolute=True)
-    # go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=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)
 
     turn_degree_v2(ctrl, msg, 90, absolute=True)
     center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
 
-    return
-
     section('任务3-2：yellow stop', "开始")
     go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
     

task_4/task_4.py

@@ -65,8 +65,9 @@ def run_task_4_back(ctrl, msg):
     go_straight(ctrl, msg, distance=1, speed=2)
 
     # Use enhanced calibration for better Y-axis correction on stone path
-    go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35, 
+    go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
-                                         mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
+    # go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35, 
+    #                                      mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
 
     section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
     # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
@@ -366,7 +367,7 @@ def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observ
                 if frame is not None:
                     # 检测轨道线 - 使用特定的石板路模式
                     center_info, _, _ = detect_dual_track_lines(
-                        frame, observe=False, save_log=False, stone_path_mode=True)
+                        frame, observe=False, save_log=False)
                     
                     # 如果成功检测到轨道线，使用它进行修正
                     if center_info is not None:

task_5/task_5.py

@@ -211,7 +211,7 @@ 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)
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
     section('任务5-1：直线移动并扫描二维码', "移动")
     
     # 最大移动距离为8米
@@ -231,7 +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)
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=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)

utils/detect_dual_track_lines.py

@@ -9,7 +9,8 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin
 def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
                             min_slope_threshold=0.4,
                             min_line_length=0.05,
-                            max_line_gap=40):
+                            max_line_gap=40,
+                            detect_height=0.4):
     """
     检测左右两条平行的黄色轨道线，优化后能够更准确处理各种路况
     
@@ -72,7 +73,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
         cv2.waitKey(delay)
     
     # 裁剪底部区域重点关注近处的黄线
-    bottom_roi_height = int(height * 0.4)  # 增加关注区域到图像底部60%
+    bottom_roi_height = int(height * detect_height)  # 增加关注区域到图像底部60%
     bottom_roi = mask[height-bottom_roi_height:, :]
     
     if observe:
@@ -544,11 +545,11 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
                     
                     # 计算拟合误差
                     predicted_xs = poly_func(center_ys)
-                    error = np.mean(np.abs(predicted_xs - center_xs))
+                    err = np.mean(np.abs(predicted_xs - center_xs))
                     
                     # 如果误差更小，更新最佳拟合
-                    if error < best_error:
+                    if err < best_error:
-                        best_error = error
+                        best_error = err
                         best_poly_coeffs = poly_coeffs
                 except:
                     continue