From 8e172759ac9ef78bd6bd3360e621266f6d6054b2 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Wed, 28 May 2025 02:12:00 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BB=BB=E5=8A=A14=EF=BC=8C?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=9B=B4=E7=BA=BF=E5=89=8D=E8=BF=9B=E8=87=B3?= =?UTF-8?q?=E9=BB=84=E7=BA=BF=E7=9A=84=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 在task_4.py中引入go_straight_until_hori_line函数，控制机器人直线前进直到检测到黄线并停在指定距离。 - 调整任务4的执行流程，确保机器人能够顺利完成新的移动任务。 - 优化了相关参数设置，以提高任务执行的准确性和稳定性。 --- base_move/move_base_hori_line.py | 193 +++++++++++++++++++++++++++++++ main.py | 7 +- task_4/task_4.py | 27 +++-- 3 files changed, 212 insertions(+), 15 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 6b6d053..d8ead2c 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -1335,3 +1335,196 @@ def follow_left_side_track(ctrl, msg, min_distance=600, max_distance=650, speed= warning("无法检测最终位置的左侧轨迹线", "警告") return False +def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0, + check_interval=0.5, speed=0.3, detect_func_version=1, + mode=11, gait_id=26, step_height=[0.06, 0.06], + observe=False): + """ + 控制机器人直线前进，直到检测到前方的横向黄线并到达指定距离 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + target_distance: 目标与横向线的距离(米)，默认为0.5米 + max_distance: 最大前进距离(米)，防止无限前进，默认为5.0米 + check_interval: 检查横向线的时间间隔(秒)，默认为0.5秒 + speed: 移动速度(米/秒)，默认为0.3米/秒 + detect_func_version: 使用的横向线检测函数版本，默认为1 + mode: 机器人运动模式，默认为11(Locomotion模式) + gait_id: 步态ID，默认为26(自变频步态) + step_height: 抬腿高度，默认为[0.06, 0.06] + observe: 是否输出中间状态信息和可视化结果，默认为False + + 返回: + bool: 是否成功找到横向线并到达目标距离 + """ + section("开始直线前进寻找横向线", "寻线") + + # 设置移动命令基本参数 + msg.mode = mode + msg.gait_id = gait_id + msg.step_height = step_height + msg.vel_des = [speed, 0, 0] # 前进速度 + msg.duration = 0 # wait next cmd + msg.life_count += 1 + + # 记录起始位置 + start_position = list(ctrl.odo_msg.xyz) + if observe: + debug(f"起始位置: {start_position}", "位置") + # 在起点放置绿色标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) + + # 发送命令开始移动 + ctrl.Send_cmd(msg) + + # 初始化变量 + total_distance = 0 + line_detected = False + last_check_time = time.time() + start_time = time.time() + + # 设置相机高度 + camera_height = 0.355 # 单位: 米 + + while total_distance < max_distance: + # 计算已移动距离 + current_position = ctrl.odo_msg.xyz + dx = current_position[0] - start_position[0] + dy = current_position[1] - start_position[1] + total_distance = math.sqrt(dx*dx + dy*dy) + + # 每隔指定时间检查一次 + current_time = time.time() + if current_time - last_check_time >= check_interval: + last_check_time = current_time + + # 获取当前图像 + image = ctrl.image_processor.get_current_image() + + # 根据指定版本检测横向线 + if detect_func_version == 1: + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True) + elif detect_func_version == 2: + 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) + 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 + + # 计算当前距离 + current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe) + + if current_distance is None: + warning("无法计算到横向线的距离，继续前进", "警告") + continue + + if observe: + info(f"检测到横向线，当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离") + + # 判断是否达到目标距离 + if current_distance <= target_distance + 0.1: # 允许0.1米的误差 + success(f"已达到目标距离，当前距离: {current_distance:.3f}米", "完成") + break + + # 如果距离比目标距离小，需要后退 + if current_distance < target_distance - 0.1: + warning(f"过度接近横向线，当前距离: {current_distance:.3f}米，需要后退", "警告") + # 平滑停止当前移动 + ctrl.base_msg.stop() + time.sleep(0.5) + + # 计算需要后退的距离 + back_distance = target_distance - current_distance + + # 设置后退命令 + msg.vel_des = [-speed/2, 0, 0] # 降低后退速度 + msg.life_count += 1 + ctrl.Send_cmd(msg) + + # 后退指定时间 + back_time = back_distance / (speed/2) + time.sleep(back_time) + + # 停止后退 + ctrl.base_msg.stop() + + if observe: + info(f"后退完成，预计后退距离: {back_distance:.3f}米", "后退") + + # 重新检查距离 + time.sleep(0.5) # 等待稳定 + image = ctrl.image_processor.get_current_image() + if detect_func_version == 1: + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True) + elif detect_func_version == 2: + 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) + + if edge_point is not None and edge_info is not None: + current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe) + if current_distance is not None: + if observe: + info(f"后退后的距离: {current_distance:.3f}米", "距离") + + break + else: + if observe and time.time() % 2 < 0.05: # 每2秒左右打印一次 + info(f"未检测到横向线，继续前进，已移动: {total_distance:.3f}米", "搜索") + + # 定期显示移动距离 + if observe and time.time() % 1 < 0.05: # 每1秒左右打印一次 + debug(f"已移动: {total_distance:.3f}米, 最大距离: {max_distance:.3f}米", "移动") + + # 短暂延时以降低CPU使用率 + time.sleep(0.05) + + # 停止移动 + ctrl.base_msg.stop() + + # 移动完成后再次检查 + if line_detected: + # 等待稳定 + time.sleep(0.5) + + # 最终检查 + image = ctrl.image_processor.get_current_image() + if detect_func_version == 1: + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True) + elif detect_func_version == 2: + 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) + + if edge_point is not None and edge_info is not None: + final_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe) + + if final_distance is not None: + if observe: + success(f"最终距离: {final_distance:.3f}米, 目标距离: {target_distance:.3f}米", "结果") + + # 判断是否达到目标 + is_success = abs(final_distance - target_distance) < 0.2 # 误差在20cm内算成功 + + if observe: + if is_success: + success(f"成功达到目标距离，误差: {abs(final_distance - target_distance):.2f}米", "成功") + else: + warning(f"未能精确达到目标距离，误差: {abs(final_distance - target_distance):.2f}米", "警告") + + return is_success + + if not line_detected: + if observe: + error(f"在最大距离({max_distance}米)内未检测到横向线", "失败") + return False + + # 如果没有进行最终检查，但检测到过横向线，默认返回成功 + return True + diff --git a/main.py b/main.py index 81a694a..773438f 100644 --- a/main.py +++ b/main.py @@ -43,10 +43,11 @@ def main(): Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # time.sleep(100) # TEST - # run_task_1(Ctrl, msg) + run_task_1(Ctrl, msg) - run_task_2(Ctrl, msg) - # run_task_2_5(Ctrl, msg) + res = run_task_2(Ctrl, msg) + + run_task_2_5(Ctrl, msg) # run_task_4(Ctrl, msg) diff --git a/task_4/task_4.py b/task_4/task_4.py index f39fc62..e7f58e0 100644 --- a/task_4/task_4.py +++ b/task_4/task_4.py @@ -11,6 +11,7 @@ from base_move.turn_degree import turn_degree from base_move.go_straight import go_straight 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 task_4.pass_bar import pass_bar @@ -30,17 +31,19 @@ def run_task_4(ctrl, msg): section('任务4-2：通过栏杆', "移动") pass_bar(ctrl, msg) - section('任务4-3：直线移动 - 石板路', "移动") - # 设置机器人运动模式为快步跑 - msg.mode = 11 # 运动模式 - msg.gait_id = 3 # 步态ID（快步跑） - msg.vel_des = [0.35, 0, 0] # 期望速度 - msg.pos_des = [ 0, 0, 0] - msg.duration = 0 # 零时长表示持续运动，直到接收到新命令 - msg.step_height = [0.21, 0.21] # 持续运动时摆动腿的离地高度 - msg.life_count += 1 - ctrl.Send_cmd(msg) - time.sleep(5000) # 持续5秒钟 + section('任务4-3：前进直到遇到黄线 - 石板路', "移动") + # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处 + go_straight_until_hori_line( + ctrl, + msg, + target_distance=0.5, # 目标与黄线的距离(米) + max_distance=8.0, # 最大搜索距离(米) + speed=0.35, # 移动速度(米/秒) + mode=11, # 运动模式 + gait_id=3, # 步态ID（快步跑） + step_height=[0.21, 0.21], # 摆动腿离地高度 + observe=True # 显示调试信息 + ) def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=10, speed=0.3): @@ -134,7 +137,7 @@ def run_task_4_back(ctrl, msg): msg.step_height = [0.21, 0.21] # 持续运动时摆动腿的离地高度 msg.life_count += 1 ctrl.Send_cmd(msg) - time.sleep(5000) # 持续5秒钟 + time.sleep(5) # 持续5秒钟 section('任务4-2：移动直到灰色天空比例小于阈值', "天空检测") go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)