✨ 在 move_base_hori_line.py 中新增 go_straight_by_hori_line 函数，封装了基于横向线的直线移动逻辑。更新 task_1.py 以集成新功能，优化任务执行流程，增强 QR 码扫描的处理和结果返回格式，提升代码可读性和可维护性。
This commit is contained in:
parent
6d41ffa96c
commit
d7675093d1
@ -8,6 +8,7 @@ import os
|
||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
from utils.detect_track import detect_horizontal_track_edge
|
||||
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
|
||||
|
||||
# 创建此模块特定的日志记录器
|
||||
@ -206,6 +207,29 @@ def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0
|
||||
|
||||
return final_distance
|
||||
|
||||
def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
|
||||
"""
|
||||
根据横向线位置，控制机器人移动到指定距离
|
||||
# INFO 本质就是封装了一下 go-straight，但是需要先校准到水平
|
||||
|
||||
参数:
|
||||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||||
distance: 目标位置与横向线的距离(米)，默认为0.5米
|
||||
speed: 移动速度(米/秒)，默认为0.5米/秒
|
||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||
"""
|
||||
# 首先校准到水平
|
||||
section("校准到横向线水平", "校准")
|
||||
aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
|
||||
|
||||
if not aligned:
|
||||
error("无法校准到横向线水平，停止移动", "停止")
|
||||
return False
|
||||
|
||||
go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe)
|
||||
|
||||
|
||||
def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
"""
|
||||
控制机器人校准并移动到横向线前的指定距离
|
||||
@ -379,8 +403,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
error("无法校准到横向线水平，停止动作", "停止")
|
||||
if scan_qrcode:
|
||||
ctrl.image_processor.stop_async_scan()
|
||||
return False, None
|
||||
return False
|
||||
return False, res
|
||||
return False, res
|
||||
else:
|
||||
info("跳过对准步骤", "信息")
|
||||
|
||||
@ -391,8 +415,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
error("无法检测到横向线，停止动作", "停止")
|
||||
if scan_qrcode:
|
||||
ctrl.image_processor.stop_async_scan()
|
||||
return False, qr_result
|
||||
return False
|
||||
return False, res
|
||||
return False, res
|
||||
|
||||
camera_height = 0.355 # 单位: 米
|
||||
r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||||
@ -400,25 +424,17 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
error("无法计算到横向线的距离，停止动作", "停止")
|
||||
if scan_qrcode:
|
||||
ctrl.image_processor.stop_async_scan()
|
||||
return False, qr_result
|
||||
return False
|
||||
return False, res
|
||||
return False, res
|
||||
|
||||
# 减去目标距离
|
||||
# 减去目标距离
|
||||
r -= target_distance
|
||||
res['radius'] = r
|
||||
|
||||
if observe:
|
||||
info(f"当前距离: {r:.3f}米", "距离")
|
||||
|
||||
# 3. 计算圆弧运动参数
|
||||
# 根据角度大小调整时间补偿系数
|
||||
if angle_deg <= 70:
|
||||
time_compensation = 0.78 # 对70度及以下角度使用更小的补偿系数
|
||||
elif angle_deg <= 90:
|
||||
time_compensation = 0.85 # 90度左右使用稍大的系数
|
||||
else:
|
||||
# 对180度旋转，增加补偿系数，使旋转时间更长
|
||||
time_compensation = 1.4 # 增加40%的时间
|
||||
|
||||
# 调整实际目标角度，针对不同角度应用不同的缩放比例
|
||||
actual_angle_deg = angle_deg
|
||||
if angle_deg <= 70:
|
||||
@ -430,16 +446,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
if observe and actual_angle_deg != angle_deg:
|
||||
info(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度", "角度")
|
||||
|
||||
|
||||
angle_rad = math.radians(actual_angle_deg)
|
||||
|
||||
# 设定角速度（rad/s），可根据实际调整
|
||||
# 减小基础角速度，增加精度
|
||||
# if angle_deg >= 170:
|
||||
# base_w = 0.5 # 大角度旋转时使用更小的基础角速度，提高精度
|
||||
# else:
|
||||
base_w = 0.8 # 原来是0.6
|
||||
|
||||
base_w = 0.8
|
||||
w = base_w if left else -base_w # 左转为正，右转为负
|
||||
|
||||
# 确保r有一个最小值，避免速度过低
|
||||
@ -456,12 +467,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
t = abs(angle_rad / w) # 运动时间，取绝对值保证为正
|
||||
|
||||
# 应用时间补偿系数
|
||||
t *= time_compensation
|
||||
|
||||
if observe:
|
||||
debug(f"圆弧半径: {effective_r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s", "计算")
|
||||
debug(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s", "时间")
|
||||
debug(f"理论运动时间: {t:.2f}s", "时间")
|
||||
|
||||
# 4. 发送圆弧运动指令
|
||||
msg.mode = 11
|
||||
@ -477,41 +485,36 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
start_yaw = ctrl.odo_msg.rpy[2]
|
||||
start_position = list(ctrl.odo_msg.xyz)
|
||||
|
||||
# 在起点放置蓝色标记
|
||||
# TEST 获取当前姿态
|
||||
yaw = ctrl.odo_msg.rpy[2]
|
||||
ctrl.place_marker(start_position[0], start_position[1],
|
||||
start_position[2] if len(start_position) > 2 else 0.0,
|
||||
'blue', observe=True)
|
||||
info(f"在起点位置放置蓝色标记: {start_position}", "位置")
|
||||
# DEBUG 在起点放置蓝色标记
|
||||
if observe:
|
||||
yaw = ctrl.odo_msg.rpy[2]
|
||||
ctrl.place_marker(start_position[0], start_position[1],
|
||||
start_position[2] if len(start_position) > 2 else 0.0,
|
||||
'blue', observe=True)
|
||||
info(f"在起点位置放置蓝色标记: {start_position}", "位置")
|
||||
|
||||
# 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
|
||||
# 圆心与机器人当前位置的距离就是圆弧半径
|
||||
center_x = start_position[0] - effective_r * math.sin(yaw) * (1 if left else -1)
|
||||
center_y = start_position[1] + effective_r * math.cos(yaw) * (1 if left else -1)
|
||||
center_z = start_position[2] if len(start_position) > 2 else 0.0
|
||||
# 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
|
||||
# 圆心与机器人当前位置的距离就是圆弧半径
|
||||
center_x = start_position[0] - effective_r * math.sin(yaw) * (1 if left else -1)
|
||||
center_y = start_position[1] + effective_r * math.cos(yaw) * (1 if left else -1)
|
||||
center_z = start_position[2] if len(start_position) > 2 else 0.0
|
||||
|
||||
# 在圆心放置绿色标记
|
||||
if observe and hasattr(ctrl, 'place_marker'):
|
||||
# 在圆心放置绿色标记
|
||||
ctrl.place_marker(center_x, center_y, center_z, 'green', observe=True)
|
||||
info(f"在旋转圆心放置绿色标记: [{center_x:.3f}, {center_y:.3f}, {center_z:.3f}]", "位置")
|
||||
|
||||
# 计算目标点的位置 - 旋转后的位置
|
||||
# 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置
|
||||
target_angle = yaw + (angle_rad if left else -angle_rad)
|
||||
# 计算从圆心到目标点的向量
|
||||
target_x = center_x + effective_r * math.sin(target_angle) * (1 if left else -1)
|
||||
target_y = center_y - effective_r * math.cos(target_angle) * (1 if left else -1)
|
||||
target_z = start_position[2] if len(start_position) > 2 else 0.0
|
||||
|
||||
# 在目标点放置红色标记
|
||||
if observe and hasattr(ctrl, 'place_marker'):
|
||||
# 计算目标点的位置 - 旋转后的位置
|
||||
# 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置
|
||||
target_angle = yaw + (angle_rad if left else -angle_rad)
|
||||
# 计算从圆心到目标点的向量
|
||||
target_x = center_x + effective_r * math.sin(target_angle) * (1 if left else -1)
|
||||
target_y = center_y - effective_r * math.cos(target_angle) * (1 if left else -1)
|
||||
target_z = start_position[2] if len(start_position) > 2 else 0.0
|
||||
|
||||
# 在目标点放置红色标记
|
||||
ctrl.place_marker(target_x, target_y, target_z, 'red', observe=True)
|
||||
info(f"在目标位置放置红色标记: [{target_x:.3f}, {target_y:.3f}, {target_z:.3f}]", "位置")
|
||||
|
||||
# 计算圆弧长度
|
||||
arc_length = abs(angle_rad * effective_r)
|
||||
|
||||
# 进入循环，实时判断
|
||||
distance_moved = 0
|
||||
angle_turned = 0
|
||||
@ -700,10 +703,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
if observe:
|
||||
info(f"旋转过程结束，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]", "停止")
|
||||
debug(f'{math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
|
||||
debug(f'旋转结束角度: {math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
|
||||
ctrl.base_msg.stop()
|
||||
if observe:
|
||||
debug(f'{math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
|
||||
debug(f'停止角度: {math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
|
||||
|
||||
# 记录停止前的角度
|
||||
pre_stop_yaw = ctrl.odo_msg.rpy[2]
|
||||
@ -737,6 +740,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
final_angle_turned += 2 * math.pi
|
||||
|
||||
final_angle_deg = math.degrees(final_angle_turned)
|
||||
|
||||
# 这里使用原始目标角度进行比较
|
||||
if not bad_big_angle_corret:
|
||||
target_angle_deg = angle_deg if left else -angle_deg
|
||||
@ -846,15 +850,15 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
# 停止异步扫描
|
||||
ctrl.image_processor.stop_async_scan()
|
||||
|
||||
# 将QR码结果添加到res字典中
|
||||
res['qr_result'] = qr_result
|
||||
|
||||
if observe:
|
||||
success("圆弧转弯完成", "完成")
|
||||
|
||||
# 根据scan_qrcode参数返回不同格式的结果
|
||||
if scan_qrcode:
|
||||
return True, qr_result
|
||||
else:
|
||||
return True
|
||||
# 统一返回结果格式
|
||||
return True, res
|
||||
|
||||
# 用法示例
|
||||
if __name__ == "__main__":
|
||||
|
||||
@ -5,10 +5,13 @@ import math
|
||||
|
||||
# 添加父目录到路径，以便能够导入utils
|
||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line, align_to_horizontal_line
|
||||
from utils.detect_track import detect_horizontal_track_edge
|
||||
from base_move.move_base_hori_line import calculate_distance_to_line
|
||||
from base_move.move_base_hori_line import (
|
||||
arc_turn_around_hori_line,
|
||||
go_straight_by_hori_line,
|
||||
move_to_hori_line
|
||||
)
|
||||
from base_move.go_straight import go_straight
|
||||
from base_move.turn_degree import turn_degree
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
@ -23,7 +26,7 @@ def run_task_1(ctrl, msg):
|
||||
# v2
|
||||
section('任务1-1：转弯并扫描QR码', "移动")
|
||||
# 在 arc_turn_around_hori_line 中启用 QR 码扫描
|
||||
turn_success, qr_result = arc_turn_around_hori_line(
|
||||
success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=85,
|
||||
@ -33,8 +36,8 @@ def run_task_1(ctrl, msg):
|
||||
qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果
|
||||
)
|
||||
|
||||
if qr_result:
|
||||
success(f"在任务1-1中成功扫描到QR码: {qr_result}", "扫描")
|
||||
if res and res['qr_result']:
|
||||
success(f"在任务1-1中成功扫描到QR码: {res['qr_result']}", "扫描")
|
||||
else:
|
||||
warning("任务1-1中未能扫描到任何QR码", "警告")
|
||||
|
||||
@ -43,14 +46,13 @@ def run_task_1(ctrl, msg):
|
||||
move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)
|
||||
|
||||
section('任务1-3：转弯', "旋转")
|
||||
# direction = True if qr_result == 'A-1' else False
|
||||
# TODO
|
||||
turn_success = arc_turn_around_hori_line(
|
||||
direction = True if res['qr_result'] == 'A-1' else False
|
||||
success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=170,
|
||||
target_distance=0.4, # TODO 优化这里的参数
|
||||
left=False, # direction,
|
||||
left=direction,
|
||||
pass_align=True,
|
||||
observe=observe,
|
||||
# TODO clear
|
||||
@ -59,8 +61,11 @@ def run_task_1(ctrl, msg):
|
||||
|
||||
section('任务1-4：直线移动', "移动")
|
||||
move_distance = 0.4
|
||||
go_straight(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
|
||||
# move_to_hori_line(ctrl, msg, target_distance=0.6, observe=observe)
|
||||
if direction:
|
||||
# TODO 这里的校准不一定好用，需要优化
|
||||
go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
|
||||
else:
|
||||
go_straight(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
|
||||
|
||||
section('任务1-5：模拟装货', "停止")
|
||||
info('机器人躺下，模拟装货过程', "信息")
|
||||
@ -74,7 +79,29 @@ def run_task_1(ctrl, msg):
|
||||
ctrl.base_msg.stand_up()
|
||||
|
||||
section('任务1-6：返回', "移动")
|
||||
go_straight(ctrl, msg, distance=-move_distance + 0.1, speed=0.5, observe=observe)
|
||||
go_straight(ctrl, msg, distance=-move_distance + res['radius'], speed=0.5, observe=observe)
|
||||
go_straight(ctrl, msg, distance=-move_distance - res['radius'], speed=0.5, observe=observe)
|
||||
|
||||
# turn and back
|
||||
turn_degree(ctrl, msg, degree=-90, absolute=True)
|
||||
|
||||
section('任务1-7：90度转弯', "旋转")
|
||||
success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=-85,
|
||||
left=True,
|
||||
observe=observe
|
||||
)
|
||||
|
||||
section('任务1-8：直线移动', "移动")
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
|
||||
|
||||
section('任务1-9：90度旋转', "旋转")
|
||||
turn_degree(ctrl, msg, degree=90, absolute=True)
|
||||
|
||||
section('任务1-10: y校准，准备 task-2', "移动")
|
||||
# TODO
|
||||
|
||||
success("任务1完成", "完成")
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user