From ce369bf71867667b14f0cfa76127b2dd2560df92 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Mon, 26 May 2025 15:06:08 +0000 Subject: [PATCH] refactor(main): integrate go_to_xy_example function for coordinate navigation - Add run_go_to_xy_example function call in main.py for enhanced navigation capabilities. - Comment out run_task_1 to streamline task execution flow. --- base_move/go_to_xy.py | 268 ++++++++++++++++++++++++++++++++++ main.py | 6 +- task_test/go_to_xy_example.py | 105 +++++++++++++ 3 files changed, 378 insertions(+), 1 deletion(-) create mode 100644 base_move/go_to_xy.py create mode 100644 task_test/go_to_xy_example.py diff --git a/base_move/go_to_xy.py b/base_move/go_to_xy.py new file mode 100644 index 0000000..9075b94 --- /dev/null +++ b/base_move/go_to_xy.py @@ -0,0 +1,268 @@ +import math +import time +import sys +import os + +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +from utils.localization_lcmt import localization_lcmt +from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing +from base_move.turn_degree import turn_degree, turn_degree_twice +from base_move.go_straight import go_straight + +# 创建本模块特定的日志记录器 +logger = get_logger("坐标移动") + +def go_to_xy(ctrl, msg, target_x, target_y, speed=0.5, precision=True, observe=False): + """ + 控制机器人移动到指定的(x,y)坐标位置，使用直接xy速度控制 + + 参数: + 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('目标点导航', "开始") + 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_distance = distance + stall_count = 0 + + 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] + + # 计算新的位移向量和距离 + 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 + if abs(distance_change) < 0.005: # 变化小于5毫米 + stall_count += 1 + if stall_count > 20: # 连续多次检测到几乎没有移动 + if observe: + warning("检测到机器人可能卡住，调整策略", "移动") + # 可能是卡住了，稍微增加速度 + actual_speed = min(actual_speed * 1.2, max_speed) + stall_count = 0 + 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 + + # 更新速度命令 + msg.vel_des = [vel_x, vel_y, 0] # [前进速度, 侧向速度, 角速度] + msg.duration = 0 # 持续执行直到下一个命令 + msg.life_count += 1 + 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 + + 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 observe: + if nav_success: + success(f"成功到达目标点，误差: {final_distance:.3f}米", "成功") + else: + warning(f"未能精确到达目标点，误差: {final_distance:.3f}米", "警告") + + return nav_success + +def go_to_xy_with_correction(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'导航尝试 {attempt}/{max_attempts}', "开始") + + # 执行基本导航 + success = go_to_xy(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.2: # 20厘米以内 + if observe: + info(f"距离目标很近 ({distance:.3f}米)，使用更高精度方式移动", "策略") + # 使用更慢的速度和更高的精度 + speed = speed * 0.6 + precision = True + + 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 + +# 用法示例 +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 diff --git a/main.py b/main.py index 2d61e01..4ef7e46 100644 --- a/main.py +++ b/main.py @@ -24,6 +24,7 @@ from task_1.task_1 import run_task_1 from task_2_5.task_2_5 import run_task_2_5 from task_4.task_4 import run_task_4 from task_test.task_left_line import run_task_test +from task_test.go_to_xy_example import run_go_to_xy_example pass_marker = True @@ -39,13 +40,16 @@ def main(): Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # time.sleep(100) # TEST - run_task_1(Ctrl, msg) + # run_task_1(Ctrl, msg) # run_task_2_5(Ctrl, msg) # run_task_4(Ctrl, msg) # run_task_test(Ctrl, msg) + + # 坐标导航示例 + run_go_to_xy_example(Ctrl, msg) # time.sleep(100) diff --git a/task_test/go_to_xy_example.py b/task_test/go_to_xy_example.py new file mode 100644 index 0000000..6d93013 --- /dev/null +++ b/task_test/go_to_xy_example.py @@ -0,0 +1,105 @@ +import time +import sys +import os + +# 添加父目录到路径，以便能够导入相关模块 +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +from base_move.go_to_xy import go_to_xy, go_to_xy_with_correction +from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing + +# 创建本模块特定的日志记录器 +logger = get_logger("坐标导航示例") + +def run_go_to_xy_example(ctrl, msg): + """ + 坐标导航示例任务：展示如何使用go_to_xy功能实现精确导航 + + 参数: + ctrl: Robot_Ctrl 对象 + msg: robot_control_cmd_lcmt 对象 + """ + section('坐标导航示例任务', "启动") + info('开始执行坐标导航示例...', "启动") + + + try: + # 示例1：简单导航到一个目标点 + section('示例1：简单导航', "开始") + target_x, target_y = 0.8, 0.2 # 前进1米 + info(f"移动到坐标点: ({target_x}, {target_y})", "目标") + + nav_success = go_to_xy(ctrl, msg, target_x, target_y, speed=0.5, observe=True) + + if nav_success: + success("成功到达目标点1", "完成") + else: + warning("未能精确到达目标点1", "警告") + + return + # 等待2秒 + time.sleep(2) + + # 示例2：使用自动修正的方式导航到目标点 + section('示例2：带修正的导航', "开始") + target_x, target_y = 0.0, 1.0 # 左转并前进1米 + info(f"移动到坐标点: ({target_x}, {target_y})", "目标") + + nav_success = go_to_xy_with_correction( + ctrl, msg, target_x, target_y, + speed=0.4, precision=True, max_attempts=2, observe=True + ) + + if nav_success: + success("成功到达目标点2", "完成") + else: + warning("未能精确到达目标点2", "警告") + + # 等待2秒 + time.sleep(2) + + # 示例3：使用坐标导航绕行矩形路径 + section('示例3：矩形路径导航', "开始") + info("执行矩形路径导航", "路径") + + # 定义矩形四个顶点的坐标 + rectangle_points = [ + (0.0, 0.0), # 起点/终点 + (1.0, 0.0), # 右侧点 + (1.0, 1.0), # 右上角点 + (0.0, 1.0), # 左上角点 + ] + + # 依次导航到各个顶点 + for i, (point_x, point_y) in enumerate(rectangle_points): + section(f'导航到矩形顶点 {i+1}/4', "移动") + info(f"目标坐标: ({point_x}, {point_y})", "目标") + + nav_success = go_to_xy_with_correction( + ctrl, msg, point_x, point_y, + speed=0.5, precision=True, observe=True + ) + + if nav_success: + success(f"成功到达顶点 {i+1}", "完成") + else: + warning(f"未能精确到达顶点 {i+1}", "警告") + + # 每个点之间短暂停顿 + time.sleep(1) + + # 示例完成，回到起点 + section('示例任务完成', "结束") + info("坐标导航示例任务完成", "完成") + + # 复位里程计（可选） + # ctrl.odo_reset() + + except Exception as e: + error(f"执行过程中发生错误: {e}", "错误") + finally: + # 确保机器人停止 + ctrl.base_msg.stop_smooth() + +if __name__ == "__main__": + # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象 + pass \ No newline at end of file