429 lines
17 KiB
Python
429 lines
17 KiB
Python
import math
|
||
import time
|
||
import cv2
|
||
|
||
import sys
|
||
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
|
||
|
||
def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||
"""
|
||
控制机器人旋转到横向线水平的位置
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
max_attempts: 最大尝试次数，默认为3次
|
||
|
||
返回:
|
||
bool: 是否成功校准
|
||
"""
|
||
attempts = 0
|
||
aligned = False
|
||
image = ctrl.image_processor.get_current_image()
|
||
|
||
while attempts < max_attempts and not aligned:
|
||
# 检测横向线边缘
|
||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0)
|
||
|
||
if edge_point is None or edge_info is None:
|
||
print("未检测到横向线，无法进行校准")
|
||
return False
|
||
|
||
# 获取检测到的斜率和其他信息
|
||
slope = edge_info["slope"]
|
||
is_horizontal = edge_info["is_horizontal"]
|
||
|
||
if observe:
|
||
print(f"检测到横向线，斜率: {slope:.6f}")
|
||
print(f"是否足够水平: {is_horizontal}")
|
||
|
||
# 如果已经水平，则无需旋转
|
||
if is_horizontal:
|
||
print("横向线已经水平，无需校准")
|
||
return True
|
||
|
||
# 计算需要旋转的角度
|
||
# 斜率 = tan(θ)，因此 θ = arctan(斜率)
|
||
angle_rad = math.atan(slope)
|
||
angle_deg = math.degrees(angle_rad)
|
||
|
||
# 调整角度方向
|
||
# 正的斜率意味着线条从左到右上升，需要逆时针旋转校正
|
||
# 负的斜率意味着线条从左到右下降，需要顺时针旋转校正
|
||
# 注意旋转方向: 顺时针为负角度，逆时针为正角度
|
||
angle_to_rotate = -angle_deg # 取负值使旋转方向正确
|
||
|
||
if observe:
|
||
print(f"需要旋转的角度: {angle_to_rotate:.2f}度")
|
||
|
||
# 可视化横向线和校准角度
|
||
if isinstance(image, str):
|
||
img = cv2.imread(image)
|
||
else:
|
||
img = image.copy()
|
||
|
||
height, width = img.shape[:2]
|
||
center_x = width // 2
|
||
|
||
# 画出检测到的横向线
|
||
line_length = 200
|
||
end_x = edge_point[0] + line_length
|
||
end_y = int(edge_point[1] + slope * line_length)
|
||
start_x = edge_point[0] - line_length
|
||
start_y = int(edge_point[1] - slope * line_length)
|
||
cv2.line(img, (start_x, start_y), (end_x, end_y), (0, 255, 0), 2)
|
||
|
||
# 画出水平线（目标线）
|
||
horizontal_y = edge_point[1]
|
||
cv2.line(img, (center_x - line_length, horizontal_y),
|
||
(center_x + line_length, horizontal_y), (0, 0, 255), 2)
|
||
|
||
# 标记角度
|
||
cv2.putText(img, f"当前斜率: {slope:.4f}", (10, 30),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||
cv2.putText(img, f"旋转角度: {angle_to_rotate:.2f}°", (10, 70),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||
|
||
cv2.imshow("校准旋转分析", img)
|
||
cv2.waitKey(1500 if observe else 1)
|
||
|
||
# 执行旋转
|
||
# 如果角度很小，增加一个小的偏移以确保旋转足够
|
||
if abs(angle_to_rotate) < 3.0:
|
||
angle_to_rotate *= 1.5 # 对小角度进行放大以确保效果
|
||
|
||
# 限制旋转角度，避免过度旋转
|
||
angle_to_rotate = max(-30, min(30, angle_to_rotate))
|
||
|
||
# 使用turn_degree函数执行旋转
|
||
turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False)
|
||
|
||
if observe:
|
||
print(f"旋转结果: {'成功' if turn_success else '失败'}")
|
||
|
||
# 增加尝试次数
|
||
attempts += 1
|
||
|
||
# 在旋转后重新获取图像，这里需要调用获取图像的函数
|
||
# 代码中没有提供获取实时图像的方法，假设每次外部会更新image参数
|
||
|
||
# 检查是否已经对齐
|
||
# 对于实际应用，应该在旋转后重新捕获图像并检测横向线
|
||
# 这里简单地根据旋转是否成功和旋转角度是否足够小来判断
|
||
if turn_success and abs(angle_to_rotate) < 5.0:
|
||
aligned = True
|
||
|
||
return aligned
|
||
|
||
def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0, observe=False):
|
||
"""
|
||
根据相机参数和图像中横线位置计算相机到横线的实际距离
|
||
|
||
几何模型说明:
|
||
1. 相机位于高度camera_height处，向下倾斜camera_tilt_angle_deg度
|
||
2. 图像底部对应相机视场的下边缘，横线在图像中的位置通过像素坐标确定
|
||
3. 计算相机视线到横线的角度，然后使用三角函数计算实际距离
|
||
|
||
参数:
|
||
edge_info: 边缘信息字典，包含distance_to_bottom等信息
|
||
camera_height: 相机高度(米)
|
||
camera_tilt_angle_deg: 相机向下倾斜的角度（度）
|
||
observe: 是否打印中间计算值
|
||
|
||
返回:
|
||
float: 到横向线的X轴水平距离(米)
|
||
"""
|
||
if edge_info is None or "distance_to_bottom" not in edge_info:
|
||
return None
|
||
|
||
# 1. 获取图像中交点到底部的距离(像素)
|
||
distance_in_pixels = edge_info["distance_to_bottom"]
|
||
if observe:
|
||
print(f"图像中交点到底部的像素距离: {distance_in_pixels}")
|
||
|
||
# 2. 获取相机参数
|
||
horizontal_fov_rad = 1.46608 # 水平视场角（弧度）约84度
|
||
image_height_px = 1080 # 图像高度（像素）
|
||
image_width_px = 1920 # 图像宽度（像素）
|
||
|
||
# 3. 计算垂直视场角
|
||
aspect_ratio = image_width_px / image_height_px # 宽高比
|
||
vertical_fov_rad = horizontal_fov_rad / aspect_ratio # 垂直视场角(弧度)
|
||
vertical_fov_deg = math.degrees(vertical_fov_rad) # 垂直视场角(度)
|
||
|
||
if observe:
|
||
print(f"相机参数: 水平FOV={math.degrees(horizontal_fov_rad):.1f}°, 垂直FOV={vertical_fov_deg:.1f}°")
|
||
print(f"图像尺寸: {image_width_px}x{image_height_px}, 宽高比: {aspect_ratio:.2f}")
|
||
|
||
# 4. 直接计算视线角度
|
||
|
||
# 计算图像底部到相机视场中心的角度
|
||
half_vfov_rad = vertical_fov_rad / 2
|
||
|
||
# 计算图像底部到横线的角度比例
|
||
# 比例 = 底部到横线的像素距离 / 图像总高度
|
||
pixel_ratio = distance_in_pixels / image_height_px
|
||
|
||
# 计算从图像底部到横线的角度
|
||
bottom_to_line_angle_rad = pixel_ratio * vertical_fov_rad
|
||
|
||
# 计算从相机视场中心到横线的角度
|
||
# 负值表示横线在视场中心以下，正值表示在中心以上
|
||
center_to_line_angle_rad = bottom_to_line_angle_rad - half_vfov_rad
|
||
|
||
# 考虑相机倾斜角度
|
||
# 相机向下倾斜为正值，此时视场中心相对水平线向下
|
||
camera_tilt_rad = math.radians(camera_tilt_angle_deg)
|
||
|
||
# 计算横线相对于水平面的视线角度
|
||
# 负值表示视线向下看到横线，正值表示视线向上看到横线
|
||
view_angle_rad = center_to_line_angle_rad - camera_tilt_rad
|
||
|
||
if observe:
|
||
print(f"视场角度关系:")
|
||
print(f" - 图像底部到横线角度: {math.degrees(bottom_to_line_angle_rad):.2f}°")
|
||
print(f" - 视场中心到横线角度: {math.degrees(center_to_line_angle_rad):.2f}°")
|
||
print(f" - 相机倾斜角度: {camera_tilt_angle_deg}°")
|
||
print(f" - 最终视线角度: {math.degrees(view_angle_rad):.2f}° ({'向下' if view_angle_rad < 0 else '向上'})")
|
||
|
||
# 5. 防止除零错误或异常值
|
||
# 确保视线角度不接近于0(水平视线无法确定地面交点)
|
||
min_angle_rad = 0.01 # 约0.57度
|
||
if abs(view_angle_rad) < min_angle_rad:
|
||
if observe:
|
||
print(f"视线角度过小({math.degrees(view_angle_rad):.2f}°)，使用最小角度: {math.degrees(min_angle_rad):.2f}°")
|
||
view_angle_rad = -min_angle_rad # 设为向下的最小角度
|
||
|
||
# 6. 计算水平距离
|
||
# 仅当视线向下时计算地面距离
|
||
if view_angle_rad < 0: # 视线向下
|
||
# 基本几何关系: 水平距离 = 高度 / tan(视线向下的角度)
|
||
# 注意角度为负，所以需要取负
|
||
ground_distance = camera_height / math.tan(-view_angle_rad)
|
||
|
||
if observe:
|
||
print(f"计算公式: 距离 = 相机高度({camera_height}米) / tan(|视线角度|({abs(math.degrees(view_angle_rad)):.2f}°))")
|
||
print(f"计算结果: 距离 = {ground_distance:.3f}米")
|
||
else: # 视线平行或向上，无法确定地面交点
|
||
if observe:
|
||
print(f"视线向上或水平，无法计算地面距离")
|
||
return 0.5 # 返回一个默认值
|
||
|
||
# 7. 应用校正和限制
|
||
# 可选的校正因子(通过实验校准)
|
||
correction_factor = 1.0
|
||
distance = ground_distance * correction_factor
|
||
|
||
# 设置合理的范围限制
|
||
min_distance = 0.1 # 最小距离(米)
|
||
|
||
# 限制结果在合理范围内
|
||
final_distance = max(min_distance, distance)
|
||
|
||
if observe and final_distance != distance:
|
||
print(f"应用范围限制: 原始距离 {distance:.3f}米 -> 最终距离 {final_distance:.3f}米")
|
||
elif observe:
|
||
print(f"最终距离: {final_distance:.3f}米")
|
||
|
||
return final_distance
|
||
|
||
def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||
"""
|
||
控制机器人校准并移动到横向线前的指定距离
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
target_distance: 目标位置与横向线的距离(米)，默认为0.5米
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
|
||
返回:
|
||
bool: 是否成功到达目标位置
|
||
"""
|
||
# 首先校准到水平
|
||
print("校准到横向线水平")
|
||
aligned = align_to_horizontal_line(ctrl, msg, observe=False)
|
||
|
||
if not aligned:
|
||
print("无法校准到横向线水平，停止移动")
|
||
return False
|
||
|
||
# 检测横向线
|
||
# image = cv2.imread("current_image.jpg") # TEST
|
||
image = ctrl.image_processor.get_current_image()
|
||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=False)
|
||
|
||
if edge_point is None or edge_info is None:
|
||
print("无法检测到横向线，停止移动")
|
||
return False
|
||
|
||
# 获取相机高度
|
||
camera_height = 0.355 # 单位: 米 # INFO from TF-tree
|
||
|
||
# 计算当前距离
|
||
current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||
|
||
if current_distance is None:
|
||
print("无法计算到横向线的距离，停止移动")
|
||
return False
|
||
|
||
if observe:
|
||
print(f"当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米")
|
||
|
||
# 计算需要移动的距离
|
||
distance_to_move = current_distance - target_distance
|
||
|
||
if abs(distance_to_move) < 0.05: # 如果已经很接近目标距离
|
||
print("已经达到目标距离，无需移动")
|
||
return True
|
||
|
||
# 设置移动命令
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
|
||
# 移动方向设置
|
||
forward = distance_to_move > 0 # 判断是前进还是后退
|
||
|
||
# 设置移动速度
|
||
move_speed = 1 # 米/秒
|
||
if forward:
|
||
msg.vel_des = [move_speed, 0, 0] # 设置前进速度
|
||
else:
|
||
msg.vel_des = [-move_speed, 0, 0] # 设置后退速度
|
||
|
||
# 获取起始位置
|
||
start_position = list(ctrl.odo_msg.xyz) # 转换为列表，因为xyz是元组
|
||
|
||
if observe:
|
||
print(f"起始位置: {start_position}")
|
||
print(f"开始移动: {'前进' if forward else '后退'} {abs(distance_to_move):.3f}米")
|
||
# 在起点放置绿色标记
|
||
if hasattr(ctrl, 'place_marker'):
|
||
ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True)
|
||
|
||
# 估算移动时间，但实际上会通过里程计控制
|
||
move_time = abs(distance_to_move) / move_speed
|
||
msg.duration = int((move_time + 2) * 1000) # 加一点余量，确保有足够时间移动
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 使用里程计进行实时监控移动距离
|
||
distance_moved = 0
|
||
start_time = time.time()
|
||
timeout = move_time + 1 # 超时时间设置为预计移动时间加5秒
|
||
|
||
while distance_moved < abs(distance_to_move) and time.time() - start_time < timeout:
|
||
# 计算已移动距离
|
||
current_position = ctrl.odo_msg.xyz
|
||
dx = current_position[0] - start_position[0]
|
||
dy = current_position[1] - start_position[1]
|
||
distance_moved = math.sqrt(dx*dx + dy*dy)
|
||
|
||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||
print(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米")
|
||
|
||
# 如果已经接近目标距离，准备停止
|
||
if distance_moved >= abs(distance_to_move) * 0.95:
|
||
break
|
||
|
||
time.sleep(0.05) # 小间隔检查位置
|
||
|
||
# 发送停止命令
|
||
msg.mode = 0
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
print(f"移动完成，通过里程计计算的移动距离: {distance_moved:.3f}米")
|
||
# 在终点放置红色标记
|
||
end_position = list(ctrl.odo_msg.xyz)
|
||
if hasattr(ctrl, 'place_marker'):
|
||
ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
|
||
|
||
# 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断
|
||
return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功
|
||
|
||
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False):
|
||
"""
|
||
对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: robot_control_cmd_lcmt对象
|
||
angle_deg: 旋转角度，支持90或180度
|
||
left: True为左转，False为右转
|
||
observe: 是否打印调试信息
|
||
返回:
|
||
bool: 是否成功完成动作
|
||
"""
|
||
# 1. 对准横线
|
||
print("校准到横向线水平")
|
||
aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
|
||
if not aligned:
|
||
print("无法校准到横向线水平，停止动作")
|
||
return False
|
||
|
||
# 2. 检测横线并计算距离
|
||
image = ctrl.image_processor.get_current_image()
|
||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe)
|
||
if edge_point is None or edge_info is None:
|
||
print("无法检测到横向线，停止动作")
|
||
return False
|
||
|
||
camera_height = 0.355 # 单位: 米
|
||
r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||
if r is None:
|
||
print("无法计算到横向线的距离，停止动作")
|
||
return False
|
||
|
||
if observe:
|
||
print(f"当前距离: {r:.3f}米")
|
||
|
||
# 3. 计算圆弧运动参数
|
||
angle_rad = math.radians(angle_deg)
|
||
# 设定角速度（rad/s），可根据实际调整
|
||
w = 0.4 if left else -0.4 # 左转为正，右转为负
|
||
v = w * r # 线速度
|
||
t = abs(angle_rad / w) # 运动时间
|
||
|
||
if observe:
|
||
print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s, 运动时间: {t:.2f}s")
|
||
|
||
# 4. 发送圆弧运动指令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度]
|
||
msg.duration = int((t + 1) * 1000) # 加1秒余量
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(t) # 等待运动完成
|
||
|
||
# 5. 发送停止指令
|
||
msg.mode = 0
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
print("圆弧运动完成")
|
||
|
||
return True
|
||
|
||
# 用法示例
|
||
if __name__ == "__main__":
|
||
move_to_hori_line(None, None, observe=True)
|
||
# 90度左转
|
||
arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True)
|
||
# 180度右转
|
||
arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)
|