283 lines
11 KiB
Python
283 lines
11 KiB
Python
import math
|
||
import time
|
||
import cv2
|
||
import numpy as np
|
||
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):
|
||
"""
|
||
根据边缘信息和相机高度计算到横向线的实际距离
|
||
|
||
参数:
|
||
edge_info: 边缘信息字典，包含distance_to_bottom等信息
|
||
camera_height: 相机高度(米)
|
||
|
||
返回:
|
||
float: 到横向线的估计距离(米)
|
||
"""
|
||
if edge_info is None or "distance_to_bottom" not in edge_info:
|
||
return None
|
||
|
||
# 获取交点到图像底部的像素距离
|
||
distance_in_pixels = edge_info["distance_to_bottom"]
|
||
|
||
# 计算比例系数 (需要根据实际场景标定)
|
||
# 这里使用一个简化的算法，假设像素距离与实际距离成反比；实际应用中应该进行相机标定和测试以获得更准确的参数
|
||
|
||
# 假设FOV (视场角)为84度 (1.46608弧度), 图像高度为1080像素
|
||
fov_vertical = 1.46608 / 16 * 9 # 假设16:9的图像比例计算垂直FOV
|
||
image_height = 1080 # 从robot.xacro中获取
|
||
|
||
# 计算每个像素对应的角度
|
||
angle_per_pixel = fov_vertical / image_height
|
||
|
||
# 计算交点对应的角度
|
||
angle = angle_per_pixel * distance_in_pixels
|
||
|
||
# 使用相机高度和角度计算距离 (使用正切函数)
|
||
# 距离 = 相机高度 / tan(角度)
|
||
distance = camera_height / math.tan(angle)
|
||
|
||
return max(0.0, distance) # 确保距离是正数
|
||
|
||
def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
|
||
"""
|
||
控制机器人校准并移动到横向线前的指定距离
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
target_distance: 目标与横向线的距离(米)，默认为0.1米
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
|
||
返回:
|
||
bool: 是否成功到达目标位置
|
||
"""
|
||
# 首先校准到水平
|
||
aligned = align_to_horizontal_line(ctrl, msg, observe=False)
|
||
|
||
if not aligned:
|
||
print("无法校准到横向线水平，停止移动")
|
||
return False
|
||
|
||
# 检测横向线
|
||
cv2.imwrite("current_image.jpg", ctrl.image_processor.get_current_image())
|
||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_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)
|
||
|
||
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}米")
|
||
|
||
# 估算移动时间，但实际上会通过里程计控制
|
||
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 + 5 # 超时时间设置为预计移动时间加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.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
print(f"移动完成，通过里程计计算的移动距离: {distance_moved:.3f}米")
|
||
|
||
# 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断
|
||
return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功
|
||
|
||
# 用法示例
|
||
if __name__ == "__main__":
|
||
# 这里需要替换为实际的控制器和消息对象
|
||
# ctrl = Robot_Ctrl()
|
||
# msg = robot_control_cmd_lcmt()
|
||
# image_processor = ImageProcessor()
|
||
|
||
# 使用示例图像路径
|
||
image_path = "path/to/image.jpg"
|
||
|
||
# 基本用法
|
||
# align_to_horizontal_line(ctrl, msg, image_path, observe=True)
|
||
# move_to_hori_line(ctrl, msg, image_path, target_distance=0.1, observe=True)
|
||
|
||
# 使用里程计和图像处理器进行更精确的定位
|
||
# 从图像处理器获取当前图像
|
||
# current_image = image_processor.get_image()
|
||
# move_to_hori_line(ctrl, msg, current_image, target_distance=0.1, observe=True, image_processor=image_processor)
|