优化 move_to_hori_line 和 arc_turn_around_hori_line 函数，增强 QR 码扫描功能
- 移除 move_to_hori_line 函数中的 QR 码扫描相关参数，简化函数接口 - 在 arc_turn_around_hori_line 函数中添加 QR 码扫描功能，支持在旋转过程中异步扫描 QR 码 - 更新 task_1.py 中的任务执行逻辑，确保在旋转和移动过程中能够实时获取 QR 码扫描结果 - 增强调试信息输出，便于跟踪任务执行状态
This commit is contained in:
parent
ef08a40adb
commit
9c251bc471
@ -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__":
|
||||
|
||||
@ -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
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user