diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index e1e9da1..9e2f1b1 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -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__": diff --git a/task_1/task_1.py b/task_1/task_1.py index 1ef9204..f0c5221 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -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完成", "完成")