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: