640 lines
26 KiB
Python
640 lines
26 KiB
Python
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 = (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
|
||
|
||
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
|
||
|
||
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
|
||
|
||
def go_to_y_v2(ctrl, msg, target_y, speed=0.5, precision=True, observe=False):
|
||
"""
|
||
控制机器人移动到指定的y坐标位置，使用直接y速度控制
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
target_y: 目标Y坐标(米)
|
||
speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
|
||
"""
|
||
target_x = ctrl.odo_msg.xyz[0]
|
||
return go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe)
|