From 1ce0f9e7bf74025310cfeacec6a344c068b76a6d Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sat, 31 May 2025 09:46:46 +0000 Subject: [PATCH] Update centering functionality in dual track control by adding new parameters for detection height and interval. Adjust message life count handling and improve variable management in task execution. Refactor related functions for enhanced clarity and performance. --- base_move/center_on_dual_tracks.py | 17 ++++++++++++----- main.py | 1 + task_3/task_3.py | 12 ++---------- task_4/task_4.py | 7 ++++--- task_5/task_5.py | 4 ++-- utils/detect_dual_track_lines.py | 5 +++-- 6 files changed, 24 insertions(+), 22 deletions(-) diff --git a/base_move/center_on_dual_tracks.py b/base_move/center_on_dual_tracks.py index 4972f42..b783df3 100644 --- a/base_move/center_on_dual_tracks.py +++ b/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) @@ -73,7 +78,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal # 检测双轨道线 detection_total_count += 1 - + center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True) if center_info is not None: @@ -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: diff --git a/main.py b/main.py index 4974bae..73abebf 100644 --- a/main.py +++ b/main.py @@ -193,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): diff --git a/task_3/task_3.py b/task_3/task_3.py index df103bc..83640d7 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -183,18 +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) + 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: @@ -254,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] # 记录起始高度 diff --git a/task_4/task_4.py b/task_4/task_4.py index 952d38a..f63477e 100644 --- a/task_4/task_4.py +++ b/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, - mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True) + go_straight(ctrl, msg, distance=4.5, speed=0.35, 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: diff --git a/task_5/task_5.py b/task_5/task_5.py index 2c13662..91ed948 100644 --- a/task_5/task_5.py +++ b/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) diff --git a/utils/detect_dual_track_lines.py b/utils/detect_dual_track_lines.py index c883f19..5accca5 100644 --- a/utils/detect_dual_track_lines.py +++ b/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: