From c08379804296dfc23e04db51f2d3dc1b4bdc5793 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Wed, 28 May 2025 03:02:24 +0000 Subject: [PATCH] Enhance navigation and movement functions in tasks - Updated main.py to adjust task execution flow and added print statements for debugging. - Refactored task_2.py to utilize go_to_xy_v2 for improved navigation accuracy and added an xy_flag parameter to run_task_2. - Introduced go_to_xy_with_correction_v2 and go_to_xy_v2 in go_to_xy.py for enhanced movement control and precision. - Modified task_2_5.py to incorporate turn_degree_v2 and added go_straight function for better maneuverability. --- base_move/go_to_xy.py | 373 +++++++++++++++++++++++++++++++++++++++++- main.py | 6 +- task_2/task_2.py | 19 ++- task_2_5/task_2_5.py | 9 +- 4 files changed, 386 insertions(+), 21 deletions(-) diff --git a/base_move/go_to_xy.py b/base_move/go_to_xy.py index 1072247..017beb5 100644 --- a/base_move/go_to_xy.py +++ b/base_move/go_to_xy.py @@ -258,11 +258,368 @@ def go_to_xy_with_correction(ctrl, msg, target_x, target_y, speed=0.5, precision return False -# 用法示例 -if __name__ == "__main__": - # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象 - # 移动到坐标(1.0, 2.0) - # go_to_xy(ctrl, msg, 1.0, 2.0, speed=0.5, observe=True) - # 移动到坐标(0.0, 0.0)并自动修正 - # go_to_xy_with_correction(ctrl, msg, 0.0, 0.0, speed=0.4, observe=True) - pass \ No newline at end of file +def go_to_xy_v2(ctrl, msg, target_x, target_y, speed=0.5, precision=True, observe=False): + """ + 控制机器人移动到指定的(x,y)坐标位置，使用增强的里程计监控和校正机制 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + target_x: 目标X坐标(米) + target_y: 目标Y坐标(米) + speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5 + precision: 是否使用高精度模式（更慢速度），默认为True + observe: 是否输出中间状态信息，默认为False + + 返回: + bool: 是否成功完成移动到目标位置 + """ + # 获取当前位置 + current_position = ctrl.odo_msg.xyz + current_x, current_y = current_position[0], current_position[1] + + if observe: + section('目标点导航V2', "开始") + info(f"当前位置: ({current_x:.3f}, {current_y:.3f})", "位置") + info(f"目标位置: ({target_x:.3f}, {target_y:.3f})", "目标") + # 在起点放置标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(current_x, current_y, + current_position[2] if len(current_position) > 2 else 0.0, + 'blue', observe=True) + # 在目标点放置标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(target_x, target_y, 0.0, 'green', observe=True) + + # 计算与目标点的距离 + dx = target_x - current_x + dy = target_y - current_y + distance = math.sqrt(dx*dx + dy*dy) + + # 如果已经非常接近目标，无需移动 + if distance < 0.05: # 5厘米以内视为已达到 + if observe: + success("已处于目标位置附近，无需移动", "完成") + return True + + if observe: + info(f"到目标点的距离: {distance:.3f}米", "路径") + + # 限制速度范围 + max_speed = min(max(abs(speed), 0.1), 1.0) + if precision: + max_speed = min(max_speed, 0.5) # 高精度模式下限制最大速度 + + # 根据距离动态调整速度 + if distance > 1.0: + actual_speed = max_speed + elif distance > 0.5: + actual_speed = max_speed * 0.8 + elif distance > 0.2: + actual_speed = max_speed * 0.6 + else: + actual_speed = max_speed * 0.4 + + # 设置移动命令基本参数 + msg.mode = 11 # Locomotion模式 + msg.gait_id = 26 # 自变频步态 + msg.step_height = [0.06, 0.06] # 抬腿高度 + + # 估算移动时间，但实际上会通过里程计控制 + estimated_time = distance / actual_speed + timeout = estimated_time + 5 # 增加超时时间为预计移动时间加5秒 + + # 增强的监控变量 + start_time = time.time() + position_check_interval = 0.05 # 位置检查间隔(秒) + last_check_time = start_time + last_position_x, last_position_y = current_x, current_y + last_distance = distance + stall_count = 0 + position_history = [] # 用于存储历史位置，检测轨迹偏移 + + # 轨迹偏移检测阈值 + trajectory_check_interval = 0.2 # 每0.2秒检查一次轨迹 + last_trajectory_check = start_time + max_trajectory_deviation = 0.15 # 最大允许的轨迹偏移（米） + + if observe: + info(f"开始移动，预计用时 {estimated_time:.2f}秒", "移动") + + # 监控移动到达目标点 + while time.time() - start_time < timeout: + current_time = time.time() + + # 按固定间隔检查位置，减少计算负担 + if current_time - last_check_time >= position_check_interval: + # 获取当前位置 + current_position = ctrl.odo_msg.xyz + current_x, current_y = current_position[0], current_position[1] + + # 记录位置历史用于轨迹分析 + position_history.append((current_x, current_y, current_time)) + # 只保留最近的位置历史 + if len(position_history) > 50: + position_history = position_history[-50:] + + # 计算新的位移向量和距离 + dx = target_x - current_x + dy = target_y - current_y + current_distance = math.sqrt(dx*dx + dy*dy) + + # 判断是否已经足够接近目标 + if current_distance < 0.05: # 5厘米以内视为已达到 + break + + # 计算移动进度 + progress = 1.0 - (current_distance / distance) + + # 检测是否卡住（距离变化很小） + distance_change = last_distance - current_distance + position_change = math.sqrt((current_x - last_position_x)**2 + (current_y - last_position_y)**2) + + if position_change < 0.005: # 几乎没有移动（5毫米以内） + stall_count += 1 + if stall_count > 10: # 连续多次检测到几乎没有移动 + if observe: + warning("检测到机器人可能卡住，调整策略", "移动") + # 尝试短暂后退再前进 + msg.vel_des = [-0.1, 0, 0] # 小幅后退 + msg.life_count = (msg.life_count + 1) % 127 + ctrl.Send_cmd(msg) + time.sleep(0.5) + stall_count = 0 + continue + else: + stall_count = 0 + + # 动态调整速度，接近目标时减速 + if current_distance < 0.2: # 接近目标时减速 + target_speed = max_speed * 0.3 + elif current_distance < 0.5: + target_speed = max_speed * 0.5 + else: + target_speed = actual_speed + + # 计算速度分量，归一化后乘以目标速度 + speed_factor = target_speed / current_distance if current_distance > 0 else 0 + vel_x = dx * speed_factor + vel_y = dy * speed_factor + + # 限制速度大小 + vel_magnitude = math.sqrt(vel_x*vel_x + vel_y*vel_y) + if vel_magnitude > target_speed: + scale_factor = target_speed / vel_magnitude + vel_x *= scale_factor + vel_y *= scale_factor + + # 更新速度命令 + msg.vel_des = [vel_x, vel_y, 0] # [前进速度, 侧向速度, 角速度] + msg.duration = 0 # 持续执行直到下一个命令 + msg.life_count = (msg.life_count + 1) % 127 # 防止溢出，确保life_count在int8_t范围内 + ctrl.Send_cmd(msg) + + if observe and (current_time - start_time) % 1.0 < position_check_interval: + debug(f"当前位置: ({current_x:.3f}, {current_y:.3f}), 剩余距离: {current_distance:.3f}米, 完成: {progress*100:.1f}%", "移动") + debug(f"速度: x={vel_x:.2f}, y={vel_y:.2f}", "速度") + + # 保存当前状态用于下次比较 + last_check_time = current_time + last_distance = current_distance + last_position_x, last_position_y = current_x, current_y + + time.sleep(0.01) # 小间隔检查位置 + + # 平滑停止 + if hasattr(ctrl.base_msg, 'stop_smooth'): + ctrl.base_msg.stop_smooth() + else: + ctrl.base_msg.stop() + + # 获取最终位置并计算与目标的误差 + final_position = ctrl.odo_msg.xyz + final_x, final_y = final_position[0], final_position[1] + final_dx = target_x - final_x + final_dy = target_y - final_y + final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy) + + if observe: + info(f"最终位置: ({final_x:.3f}, {final_y:.3f})", "位置") + info(f"与目标的距离误差: {final_distance:.3f}米", "误差") + # 在终点放置标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(final_x, final_y, + final_position[2] if len(final_position) > 2 else 0.0, + 'red', observe=True) + + # 判断是否成功到达目标点（误差在10厘米以内） + nav_success = final_distance < 0.1 + + # 如果误差较大但还在可接受范围内，尝试精确调整 + if not nav_success and final_distance < 0.3: + if observe: + warning(f"第一阶段导航误差为 {final_distance:.3f}米，尝试精确调整", "调整") + + # 使用直线模式进行精确调整 + msg.mode = 11 + msg.gait_id = 1 # 使用步行步态提高精度 + + # 计算新的速度分量，使用更低的速度 + dx = target_x - final_x + dy = target_y - final_y + adjust_distance = math.sqrt(dx*dx + dy*dy) + adjust_speed = min(0.2, max_speed * 0.4) # 降低速度提高精度 + + # 计算速度分量 + speed_factor = adjust_speed / adjust_distance if adjust_distance > 0 else 0 + vel_x = dx * speed_factor + vel_y = dy * speed_factor + + # 短暂精确移动 + adjust_time = min(adjust_distance / adjust_speed * 1.5, 3.0) # 最多3秒的调整时间 + adjust_start = time.time() + + while time.time() - adjust_start < adjust_time: + # 更新当前位置 + current_position = ctrl.odo_msg.xyz + current_x, current_y = current_position[0], current_position[1] + + # 重新计算距离和方向 + dx = target_x - current_x + dy = target_y - current_y + current_distance = math.sqrt(dx*dx + dy*dy) + + # 如果已经足够接近，提前结束 + if current_distance < 0.05: + break + + # 动态调整速度 + adjust_speed = min(0.2, current_distance * 0.5) + speed_factor = adjust_speed / current_distance if current_distance > 0 else 0 + vel_x = dx * speed_factor + vel_y = dy * speed_factor + + # 发送命令 + msg.vel_des = [vel_x, vel_y, 0] + msg.life_count = (msg.life_count + 1) % 127 + ctrl.Send_cmd(msg) + + if observe and (time.time() - adjust_start) % 0.5 < 0.05: + debug(f"精确调整中: 位置 ({current_x:.3f}, {current_y:.3f}), 距离: {current_distance:.3f}米", "调整") + + time.sleep(0.05) + + # 停止 + if hasattr(ctrl.base_msg, 'stop_smooth'): + ctrl.base_msg.stop_smooth() + else: + ctrl.base_msg.stop() + + # 获取最终位置并重新计算误差 + final_position = ctrl.odo_msg.xyz + final_x, final_y = final_position[0], final_position[1] + final_dx = target_x - final_x + final_dy = target_y - final_y + final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy) + + if observe: + info(f"调整后最终位置: ({final_x:.3f}, {final_y:.3f})", "位置") + info(f"调整后误差: {final_distance:.3f}米", "误差") + # 更新终点标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(final_x, final_y, + final_position[2] if len(final_position) > 2 else 0.0, + 'purple', observe=True) + + # 重新判断是否成功 + nav_success = final_distance < 0.1 + + if observe: + if nav_success: + success(f"成功到达目标点，误差: {final_distance:.3f}米", "成功") + else: + warning(f"未能精确到达目标点，误差: {final_distance:.3f}米", "警告") + + return nav_success +def go_to_xy_with_correction_v2(ctrl, msg, target_x, target_y, speed=0.5, precision=True, + max_attempts=2, observe=False): + """ + 控制机器人移动到指定的(x,y)坐标位置，使用增强的里程计监控和校正机制，并在必要时进行多次尝试 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + target_x: 目标X坐标(米) + target_y: 目标Y坐标(米) + speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5 + precision: 是否使用高精度模式，默认为True + max_attempts: 最大尝试次数，默认为2 + observe: 是否输出中间状态信息，默认为False + + 返回: + bool: 是否成功完成移动到目标位置 + """ + attempt = 1 + + while attempt <= max_attempts: + if observe: + section(f'导航尝试V2 {attempt}/{max_attempts}', "开始") + + # 执行增强版导航 + success = go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe) + + if success: + return True + + # 如果导航失败且还有尝试次数，进行修正 + if attempt < max_attempts: + if observe: + warning(f"第{attempt}次导航未达到预期精度，尝试不同策略", "修正") + + # 获取当前位置 + current_position = ctrl.odo_msg.xyz + current_x, current_y = current_position[0], current_position[1] + + # 计算与目标点的距离和角度 + dx = target_x - current_x + dy = target_y - current_y + distance = math.sqrt(dx*dx + dy*dy) + + # 距离较近时使用更精确的方式 + if distance < 0.3: # 30厘米以内 + if observe: + info(f"距离目标较近 ({distance:.3f}米)，使用精确模式移动", "策略") + speed = min(speed * 0.6, 0.3) # 降低速度 + precision = True + else: + # 距离较远时尝试用分段路径 + if observe: + info(f"距离目标较远 ({distance:.3f}米)，尝试分段导航", "策略") + # 计算中间点 + mid_x = current_x + dx * 0.5 + mid_y = current_y + dy * 0.5 + + # 先移动到中间点 + if observe: + info(f"先移动到中间点 ({mid_x:.3f}, {mid_y:.3f})", "中间点") + mid_success = go_to_xy_v2(ctrl, msg, mid_x, mid_y, speed, precision, observe) + + if not mid_success and observe: + warning("到达中间点失败，直接尝试目标点", "策略调整") + + attempt += 1 + + # 获取最终位置并计算与目标的误差 + final_position = ctrl.odo_msg.xyz + final_x, final_y = final_position[0], final_position[1] + final_dx = target_x - final_x + final_dy = target_y - final_y + final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy) + + if observe: + if final_distance < 0.2: + warning(f"虽未达到精确目标，但已在目标附近 ({final_distance:.3f}米)", "部分成功") + else: + error(f"多次尝试后仍未能达到目标点，最终误差: {final_distance:.3f}米", "失败") + + return False diff --git a/main.py b/main.py index c4d8495..ce10b3b 100644 --- a/main.py +++ b/main.py @@ -28,6 +28,7 @@ from task_test.task_left_line import run_task_test from task_5.task_5 import run_task_5 # from task_test.go_to_xy_example import run_go_to_xy_example + from base_move.turn_degree import turn_degree pass_marker = True @@ -44,10 +45,11 @@ def main(): Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # time.sleep(100) # TEST - run_task_1(Ctrl, msg) + # run_task_1(Ctrl, msg) - arrow_direction = run_task_2(Ctrl, msg) + arrow_direction = run_task_2(Ctrl, msg, xy_flag=False) + print(f"arrow_direction: {arrow_direction}") run_task_2_5(Ctrl, msg, direction=arrow_direction) if arrow_direction == 'left': diff --git a/task_2/task_2.py b/task_2/task_2.py index 148c556..5ca3b0e 100644 --- a/task_2/task_2.py +++ b/task_2/task_2.py @@ -8,7 +8,7 @@ import queue from threading import Thread, Lock sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from base_move.go_to_xy import go_to_xy +from base_move.go_to_xy import go_to_xy, go_to_xy_v2 from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection @@ -100,7 +100,7 @@ class AsyncArrowDetector: # 保存检测结果的可视化图像 timestamp = time.strftime("%Y%m%d_%H%M%S") save_path = f"{self.save_dir}/arrow_detection_{timestamp}.jpg" - visualize_arrow_detection(img, save_path=save_path) + # visualize_arrow_detection(img, save_path=save_path) info(f"箭头检测可视化结果已保存至: {save_path}", "箭头检测") except Exception as e: error(f"异步箭头检测出错: {str(e)}", "错误") @@ -115,9 +115,9 @@ class AsyncArrowDetector: with self.lock: return self.arrow_result, self.result_time, self.last_processed_image -def run_task_2(ctrl, msg): +def run_task_2(ctrl, msg, xy_flag=False): # 微调 xy 和角度 - go_to_xy(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True) + go_to_xy_v2(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True) turn_degree(ctrl, msg, 0.8, absolute=True) print('角度为',ctrl.odo_msg.rpy[2]) @@ -145,7 +145,7 @@ def run_task_2(ctrl, msg): (0.3,0,0.6,0,0,0,1), # #前走一点 # (0.3,0,0,0,0,0,0.5) - ] # [vel_x, vel_z] 对应 [左转, 右转, 左转] + ] # [vel_x, vel_z] 对应 [左转, 右转, 左转] gotoxy=[ (0.87,0.23), @@ -184,8 +184,9 @@ def run_task_2(ctrl, msg): msg.life_count += 1 ctrl.Send_cmd(msg) - # 根据转弯方向调整持续时间（假设半圆需要3秒） - go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True) + # 根据转弯方向调整持续时间（假设半圆需要3秒 + if xy_flag: + go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True) # 打印当前方向 print(f"开始 {text[i]} ") print(f"第{i}次",'角度为',ctrl.odo_msg.rpy[2]) @@ -222,14 +223,14 @@ def run_task_2(ctrl, msg): image = ctrl.image_processor.get_current_image() if image is not None: # 直接在当前图像上检测 - arrow_direction = detect_arrow_direction(image, observe=True) + arrow_direction = detect_arrow_direction(image, observe=False) info(f"直接检测到箭头方向: {arrow_direction}", "箭头检测") # 保存检测结果的可视化图像 timestamp = time.strftime("%Y%m%d_%H%M%S") save_path = f"logs/image/arrow_detection_final_{timestamp}.jpg" os.makedirs(os.path.dirname(save_path), exist_ok=True) - visualize_arrow_detection(image, save_path=save_path) + # visualize_arrow_detection(image, save_path=save_path) info(f"最终箭头检测可视化结果已保存至: {save_path}", "箭头检测") else: warning("无法获取当前图像，箭头方向检测失败", "箭头检测") diff --git a/task_2_5/task_2_5.py b/task_2_5/task_2_5.py index 685f78f..4926a2e 100644 --- a/task_2_5/task_2_5.py +++ b/task_2_5/task_2_5.py @@ -5,7 +5,8 @@ import os # 添加父目录到路径，以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from base_move.turn_degree import turn_degree +from base_move.turn_degree import turn_degree, turn_degree_v2 +from base_move.go_to_xy import go_straight from base_move.move_base_hori_line import arc_turn_around_hori_line, align_to_horizontal_line from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing @@ -14,6 +15,8 @@ observe = True def run_task_2_5(Ctrl, msg, direction='left'): section('任务2.5：预备进入任务3', "启动") + go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe) + # TEST turn_degree(Ctrl, msg, 90, absolute=observe) @@ -28,9 +31,11 @@ def run_task_2_5(Ctrl, msg, direction='left'): observe=observe, ) + go_straight(Ctrl, msg, distance=0.2, speed=0.5, observe=observe) + section('任务2.5-2：第二次旋转', "移动") - turn_degree(Ctrl, msg, degree=90, absolute=True) + turn_degree_v2(Ctrl, msg, degree=90, absolute=True) # arc_turn_around_hori_line( # Ctrl,