From 9c251bc4713d0b8faec037dee73cda12b6e2b78b Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Thu, 15 May 2025 23:42:09 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=20move=5Fto=5Fhori=5Fline=20?= =?UTF-8?q?=E5=92=8C=20arc=5Fturn=5Faround=5Fhori=5Fline=20=E5=87=BD?= =?UTF-8?q?=E6=95=B0=EF=BC=8C=E5=A2=9E=E5=BC=BA=20QR=20=E7=A0=81=E6=89=AB?= =?UTF-8?q?=E6=8F=8F=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 移除 move_to_hori_line 函数中的 QR 码扫描相关参数，简化函数接口 - 在 arc_turn_around_hori_line 函数中添加 QR 码扫描功能，支持在旋转过程中异步扫描 QR 码 - 更新 task_1.py 中的任务执行逻辑，确保在旋转和移动过程中能够实时获取 QR 码扫描结果 - 增强调试信息输出，便于跟踪任务执行状态 --- base_move/move_base_hori_line.py | 141 +++++++++++++++---------------- task_1/task_1.py | 61 ++++++------- 2 files changed, 96 insertions(+), 106 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 803d017..cdac6db 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -202,7 +202,7 @@ def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0 return final_distance -def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode=False, qr_check_interval=0.5): +def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): """ 控制机器人校准并移动到横向线前的指定距离 @@ -211,56 +211,24 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode msg: robot_control_cmd_lcmt 对象，用于发送命令 target_distance: 目标位置与横向线的距离(米)，默认为0.5米 observe: 是否输出中间状态信息和可视化结果，默认为False - scan_qrcode: 是否在移动过程中扫描QR码，默认为False - qr_check_interval: QR码检查间隔时间(秒)，默认为0.5秒 返回: - bool or tuple: 如果scan_qrcode为False，返回bool表示是否成功到达目标位置； - 如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功到达目标位置, QR码扫描结果) + bool: 是否成功到达目标位置 """ - # 启动异步QR码扫描（如果需要） - qr_result = None - if scan_qrcode: - # 确保image_processor存在 - if not hasattr(ctrl, 'image_processor') or ctrl.image_processor is None: - print("警告: 无法启用QR码扫描，image_processor不存在") - scan_qrcode = False - else: - # 启动异步扫描 - try: - ctrl.image_processor.start_async_scan(interval=0.2) - print("已启动异步QR码扫描") - except Exception as e: - print(f"启动QR码扫描失败: {e}") - scan_qrcode = False - # 首先校准到水平 print("校准到横向线水平") aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止移动") - if scan_qrcode: - ctrl.image_processor.stop_async_scan() - return False, None return False - # 校准后检查是否已经扫描到QR码 - if scan_qrcode: - qr_data, scan_time = ctrl.image_processor.get_last_qr_result() - if qr_data: - qr_result = qr_data - print(f"🔍 校准过程中已扫描到QR码: {qr_data}") - # 检测横向线 image = ctrl.image_processor.get_current_image() edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线，停止移动") - if scan_qrcode: - ctrl.image_processor.stop_async_scan() - return False, qr_result return False # 获取相机高度 @@ -271,9 +239,6 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode if current_distance is None: print("无法计算到横向线的距离，停止移动") - if scan_qrcode: - ctrl.image_processor.stop_async_scan() - return False, qr_result return False if observe: @@ -284,9 +249,6 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode if abs(distance_to_move) < 0.05: # 如果已经很接近目标距离 print("已经达到目标距离，无需移动") - if scan_qrcode: - ctrl.image_processor.stop_async_scan() - return True, qr_result return True # 设置移动命令 @@ -326,9 +288,8 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode distance_moved = 0 start_time = time.time() timeout = move_time + 1 # 超时时间设置为预计移动时间加1秒 - last_qr_check_time = 0 - # 监控移动距离，并在移动过程中检查QR码（如果启用） + # 监控移动距离，但不执行减速（改用stop_smooth） while distance_moved < abs(distance_to_move) * 0.95 and time.time() - start_time < timeout: # 计算已移动距离 current_position = ctrl.odo_msg.xyz @@ -336,16 +297,6 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode dy = current_position[1] - start_position[1] distance_moved = math.sqrt(dx*dx + dy*dy) - # 检查QR码扫描结果（如果启用） - if scan_qrcode: - current_time = time.time() - if current_time - last_qr_check_time >= qr_check_interval: - qr_data, scan_time = ctrl.image_processor.get_last_qr_result() - if qr_data and scan_time > start_time: # 确保是在移动开始后的扫描结果 - qr_result = qr_data - print(f"🔍 在移动过程中扫描到QR码: {qr_data}") - last_qr_check_time = current_time - if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 print(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米") @@ -361,26 +312,10 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False, scan_qrcode if hasattr(ctrl, 'place_marker'): ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True) - # 移动完成后再检查一次QR码扫描结果 - if scan_qrcode: - qr_data, scan_time = ctrl.image_processor.get_last_qr_result() - if qr_data and (qr_result is None or scan_time > last_qr_check_time): - qr_result = qr_data - print(f"🔍 移动完成后最终扫描到QR码: {qr_data}") - - # 停止异步扫描 - ctrl.image_processor.stop_async_scan() - - # 判断移动是否成功 - move_success = abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 - - # 根据scan_qrcode参数返回不同格式的结果 - if scan_qrcode: - return move_success, qr_result - else: - return move_success + # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断 + return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 -def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, observe=False): +def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, observe=False, scan_qrcode=False, qr_check_interval=0.3): """ 对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。 参数: @@ -390,21 +325,53 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc angle_deg: 旋转角度，支持90或180度 left: True为左转，False为右转 observe: 是否打印调试信息 + scan_qrcode: 是否在旋转过程中扫描QR码，默认为False + qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒 返回: - bool: 是否成功完成动作 + bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作； + 如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果) """ + # 启动异步QR码扫描（如果需要） + qr_result = None + if scan_qrcode: + # 确保image_processor存在 + if not hasattr(ctrl, 'image_processor') or ctrl.image_processor is None: + print("警告: 无法启用QR码扫描，image_processor不存在") + scan_qrcode = False + else: + # 启动异步扫描 + try: + ctrl.image_processor.start_async_scan(interval=0.2) + print("已启动异步QR码扫描") + except Exception as e: + print(f"启动QR码扫描失败: {e}") + scan_qrcode = False + # 1. 对准横线 print("校准到横向线水平") aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止动作") + if scan_qrcode: + ctrl.image_processor.stop_async_scan() + return False, None return False + # 校准后检查是否已经扫描到QR码 + if scan_qrcode: + qr_data, scan_time = ctrl.image_processor.get_last_qr_result() + if qr_data: + qr_result = qr_data + print(f"🔍 校准过程中已扫描到QR码: {qr_data}") + # 2. 检测横线并计算距离 image = ctrl.image_processor.get_current_image() edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线，停止动作") + if scan_qrcode: + ctrl.image_processor.stop_async_scan() + return False, qr_result return False camera_height = 0.355 # 单位: 米 @@ -415,6 +382,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc if r is None: print("无法计算到横向线的距离，停止动作") + if scan_qrcode: + ctrl.image_processor.stop_async_scan() + return False, qr_result return False if observe: @@ -478,6 +448,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc angle_turned = 0 start_time = time.time() timeout = t + 2 # 给个超时保护 + last_qr_check_time = 0 # 初始速度 current_v = v @@ -508,6 +479,16 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc while angle_turned < -math.pi: angle_turned += 2 * math.pi + # 检查QR码扫描结果（如果启用） + if scan_qrcode: + current_time = time.time() + if current_time - last_qr_check_time >= qr_check_interval: + qr_data, scan_time = ctrl.image_processor.get_last_qr_result() + if qr_data and scan_time > start_time: # 确保是在旋转开始后的扫描结果 + qr_result = qr_data + print(f"🔍 在旋转过程中扫描到QR码: {qr_data}") + last_qr_check_time = current_time + # 计算角度完成比例 completion_ratio = abs(angle_turned) / abs(angle_rad) @@ -602,10 +583,24 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc if observe: print(f"微调结果: {'成功' if turn_success else '失败'}") + # 移动完成后再检查一次QR码扫描结果 + if scan_qrcode: + qr_data, scan_time = ctrl.image_processor.get_last_qr_result() + if qr_data and (qr_result is None or scan_time > last_qr_check_time): + qr_result = qr_data + print(f"🔍 旋转完成后最终扫描到QR码: {qr_data}") + + # 停止异步扫描 + ctrl.image_processor.stop_async_scan() + if observe: print("圆弧转弯完成") - return True + # 根据scan_qrcode参数返回不同格式的结果 + if scan_qrcode: + return True, qr_result + else: + return True # 用法示例 if __name__ == "__main__": diff --git a/task_1/task_1.py b/task_1/task_1.py index 1e64991..2bd6e1d 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -16,26 +16,43 @@ def run_task_1(ctrl, msg): # v2 print('😺 task 1 - 1') - arc_turn_around_hori_line(ctrl, msg, angle_deg=85, left=False, observe=observe) - - print('😺 task 1 - 2') - # 使用内置的QR码扫描功能执行移动 - move_success, qr_result = move_to_hori_line( + # 在 arc_turn_around_hori_line 中启用 QR 码扫描 + turn_success, qr_result = arc_turn_around_hori_line( ctrl=ctrl, msg=msg, - target_distance=1, + angle_deg=85, + left=False, observe=observe, - scan_qrcode=True, # 启用QR码扫描 - qr_check_interval=0.3 # 每0.3秒检查一次QR码结果 + scan_qrcode=True, # 启用 QR 码扫描 + qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果 ) if qr_result: - print(f"🎯 成功扫描到QR码: {qr_result}") + print(f"🎯 在任务 1-1 中成功扫描到 QR 码: {qr_result}") else: - print("⚠️ 未能扫描到任何QR码") + print("⚠️ 任务 1-1 中未能扫描到任何 QR 码") + + print('😺 task 1 - 2') + # 执行常规的移动操作，不需要 QR 码扫描 + move_to_hori_line(ctrl, msg, target_distance=1, observe=observe) print('😺 task 1 - 3') - arc_turn_around_hori_line(ctrl, msg, angle_deg=180, target_distance=0.4, left=True, observe=observe) + # 在第二次旋转操作中也启用 QR 码扫描 + turn_success, qr_result = arc_turn_around_hori_line( + ctrl=ctrl, + msg=msg, + angle_deg=180, + target_distance=0.4, + left=True, + observe=observe, + scan_qrcode=True, + qr_check_interval=0.3 + ) + + if qr_result: + print(f"🎯 在任务 1-3 中成功扫描到 QR 码: {qr_result}") + else: + print("⚠️ 任务 1-3 中未能扫描到任何 QR 码") return @@ -43,26 +60,4 @@ def run_task_1(ctrl, msg): move_to_hori_line(ctrl, msg, observe=observe) -# 保留move_with_qr_scan函数作为备份，如果需要特殊处理可以使用 -def move_with_qr_scan(ctrl, msg, target_distance=0.5, observe=False): - """ - 结合移动到指定位置和 QR 码扫描功能的函数，使用异步扫描 - - 参数: - ctrl: Robot_Ctrl 对象 - msg: 机器人控制消息对象 - target_distance: 目标距离 - observe: 是否打印调试信息 - """ - # 直接使用内置的扫描功能 - return move_to_hori_line( - ctrl=ctrl, - msg=msg, - target_distance=target_distance, - observe=observe, - scan_qrcode=True, - qr_check_interval=0.3 - ) - -