465 lines
20 KiB
Python
465 lines
20 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
import cv2
|
||
import numpy as np
|
||
import math
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
|
||
from base_move.turn_degree import turn_degree, turn_degree_v2
|
||
from base_move.go_straight import go_straight, go_lateral
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
from utils.gray_sky_analyzer import analyze_gray_sky_ratio
|
||
from utils.detect_track import detect_horizontal_track_edge
|
||
from utils.detect_dual_track_lines import detect_dual_track_lines
|
||
from base_move.move_base_hori_line import calculate_distance_to_line
|
||
from task_4.pass_bar import pass_bar
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务4")
|
||
|
||
def run_task_4(ctrl, msg):
|
||
section('任务4-1：直线移动', "移动")
|
||
# 设置机器人运动模式为快步跑
|
||
msg.mode = 11 # 运动模式
|
||
msg.gait_id = 3 # 步态ID（快步跑）
|
||
msg.vel_des = [0.35, 0, 0] # 期望速度
|
||
msg.pos_des = [ 0, 0, 0]
|
||
msg.duration = 0 # 零时长表示持续运动，直到接收到新命令
|
||
msg.step_height = [0.21, 0.21] # 持续运动时摆动腿的离地高度
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(5) # 持续5秒钟
|
||
|
||
section('任务4-2：移动直到灰色天空比例小于阈值', "天空检测")
|
||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)
|
||
|
||
section('任务4-3：通过栏杆', "移动")
|
||
pass_bar(ctrl, msg)
|
||
|
||
|
||
def run_task_4_back(ctrl, msg):
|
||
"""
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制消息对象
|
||
image_processor: 可选的图像处理器实例
|
||
"""
|
||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||
# center_on_dual_tracks(ctrl, msg, max_time=30, observe=False, stone_path_mode=False)
|
||
|
||
# 向右移动0.5秒
|
||
section('任务4-回程：向右移动', "移动")
|
||
# go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True) # DEBUG
|
||
|
||
section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
|
||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5)
|
||
|
||
section('任务4-2：通过栏杆', "移动")
|
||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||
pass_bar(ctrl, msg)
|
||
|
||
section('任务4-3：stone', "移动")
|
||
go_straight(ctrl, msg, distance=1, speed=2)
|
||
|
||
# Use enhanced calibration for better Y-axis correction on stone path
|
||
go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
|
||
# go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35,
|
||
# mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
|
||
|
||
section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
|
||
# 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
|
||
# 获取相机高度
|
||
camera_height = 0.355 # 单位: 米 # INFO from TF-tree
|
||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=True, save_log=True)
|
||
current_distance = calculate_distance_to_line(edge_info, camera_height, observe=True)
|
||
go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||
|
||
|
||
def go_straight_until_sky_ratio_below(ctrl, msg,
|
||
sky_ratio_threshold=0.2,
|
||
step_distance=0.5,
|
||
max_distance=2,
|
||
speed=0.3
|
||
):
|
||
"""
|
||
控制机器人沿直线行走，直到灰色天空比例低于指定阈值
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制命令消息对象
|
||
sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例低于此值时停止
|
||
step_distance: 每次移动的步长(米)
|
||
max_distance: 最大移动距离(米)，防止无限前进
|
||
speed: 移动速度(米/秒)
|
||
|
||
返回:
|
||
bool: 是否成功找到天空比例低于阈值的位置
|
||
"""
|
||
total_distance = 0
|
||
success_flag = False
|
||
|
||
# 设置移动命令
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
|
||
while total_distance < max_distance:
|
||
# 获取当前图像
|
||
current_image = ctrl.image_processor.get_current_image()
|
||
if current_image is None:
|
||
warning("无法获取图像，等待...", "图像")
|
||
time.sleep(0.5)
|
||
continue
|
||
|
||
# 保存当前图像用于分析
|
||
temp_image_path = "/tmp/current_sky_image.jpg"
|
||
cv2.imwrite(temp_image_path, current_image)
|
||
|
||
# 分析灰色天空比例
|
||
try:
|
||
sky_ratio = analyze_gray_sky_ratio(temp_image_path)
|
||
info(f"当前灰色天空比例: {sky_ratio:.2%}", "分析")
|
||
|
||
# 如果天空比例高于阈值，停止移动
|
||
if sky_ratio < sky_ratio_threshold:
|
||
success(f"检测到灰色天空比例({sky_ratio:.2%})低于阈值({sky_ratio_threshold:.2%})，停止移动", "完成")
|
||
success_flag = True
|
||
break
|
||
|
||
except Exception as e:
|
||
error(f"分析图像时出错: {e}", "错误")
|
||
|
||
# 继续前进一段距离
|
||
info(f"继续前进 {step_distance} 米...", "移动")
|
||
|
||
# 设置移动速度和方向
|
||
msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度]
|
||
msg.duration = 0 # wait next cmd
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 估算前进时间
|
||
move_time = step_distance / speed
|
||
time.sleep(move_time)
|
||
|
||
# 累计移动距离
|
||
total_distance += step_distance
|
||
info(f"已移动总距离: {total_distance:.2f} 米", "距离")
|
||
|
||
# 平滑停止
|
||
if hasattr(ctrl.base_msg, 'stop_smooth'):
|
||
ctrl.base_msg.stop_smooth()
|
||
else:
|
||
ctrl.base_msg.stop()
|
||
|
||
if not success_flag and total_distance >= max_distance:
|
||
warning(f"已达到最大移动距离 {max_distance} 米，但未找到天空比例小于 {sky_ratio_threshold:.2%} 的位置", "超时")
|
||
|
||
return success_flag
|
||
|
||
def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observe=False,
|
||
mode=11, gait_id=3, step_height=[0.21, 0.21]):
|
||
"""
|
||
控制机器人在石板路上沿直线行走，使用视觉校准和姿态传感器融合来保持直线
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象
|
||
msg: 控制消息对象
|
||
distance: 要行走的距离(米)
|
||
speed: 行走速度(米/秒)
|
||
observe: 是否输出调试信息
|
||
mode: 运动模式
|
||
gait_id: 步态ID
|
||
step_height: 摆动腿高度
|
||
|
||
返回:
|
||
bool: 是否成功完成
|
||
"""
|
||
section("开始石板路增强直线移动", "石板路移动")
|
||
|
||
# 参数验证
|
||
if abs(distance) < 0.01:
|
||
info("距离太短，无需移动", "信息")
|
||
return True
|
||
|
||
# 检查相机是否可用
|
||
if not hasattr(ctrl, 'image_processor') or not hasattr(ctrl.image_processor, 'get_current_image'):
|
||
warning("机器人控制器没有提供图像处理器，将使用姿态传感器辅助校准", "警告")
|
||
|
||
# 限制速度范围
|
||
speed = min(max(abs(speed), 0.1), 1.0)
|
||
|
||
# 确定前进或后退方向
|
||
forward = distance > 0
|
||
move_speed = speed if forward else -speed
|
||
abs_distance = abs(distance)
|
||
|
||
# 获取起始位置和姿态
|
||
start_position = list(ctrl.odo_msg.xyz)
|
||
start_yaw = ctrl.odo_msg.rpy[2] # 记录起始朝向
|
||
|
||
if observe:
|
||
debug(f"起始位置: {start_position}", "位置")
|
||
info(f"开始石板路增强直线移动，距离: {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒", "移动")
|
||
|
||
# 设置移动命令
|
||
msg.mode = mode
|
||
msg.gait_id = gait_id
|
||
msg.step_height = step_height
|
||
msg.duration = 0 # wait next cmd
|
||
|
||
# 根据需要移动的距离动态调整移动速度
|
||
if abs_distance > 1.0:
|
||
actual_speed = move_speed # 距离较远时用设定速度
|
||
else:
|
||
actual_speed = move_speed * 0.8 # 较近距离略微降速
|
||
|
||
# 设置移动速度和方向
|
||
msg.vel_des = [actual_speed, 0, 0] # [前进速度, 侧向速度, 角速度]
|
||
msg.life_count += 1
|
||
|
||
# 发送命令
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 估算移动时间，但实际上会通过里程计控制
|
||
estimated_time = abs_distance / abs(actual_speed)
|
||
timeout = estimated_time + 5 # 增加超时时间
|
||
|
||
# 使用里程计进行实时监控移动距离
|
||
distance_moved = 0
|
||
start_time = time.time()
|
||
last_check_time = start_time
|
||
position_check_interval = 0.1 # 位置检查间隔(秒)
|
||
|
||
# 计算移动方向单位向量（世界坐标系下）
|
||
direction_vector = [math.cos(start_yaw), math.sin(start_yaw)]
|
||
if not forward:
|
||
direction_vector = [-direction_vector[0], -direction_vector[1]]
|
||
|
||
# 视觉跟踪相关变量
|
||
vision_check_interval = 0.2 # 视觉检查间隔(秒)
|
||
last_vision_check = start_time
|
||
vision_correction_gain = 0.006 # 视觉修正增益系数
|
||
|
||
# 用于滤波的队列
|
||
deviation_queue = []
|
||
filter_size = 5
|
||
last_valid_deviation = 0
|
||
|
||
# PID控制参数 - 用于角度修正
|
||
kp_angle = 0.6 # 比例系数
|
||
ki_angle = 0.02 # 积分系数
|
||
kd_angle = 0.1 # 微分系数
|
||
|
||
# PID控制变量
|
||
angle_error_integral = 0
|
||
last_angle_error = 0
|
||
|
||
# 偏移量累计 - 用于检测持续偏移
|
||
y_offset_accumulator = 0
|
||
|
||
# 动态调整参数
|
||
slow_down_ratio = 0.85 # 当移动到目标距离的85%时开始减速
|
||
completion_threshold = 0.95 # 当移动到目标距离的95%时停止
|
||
|
||
# 监控移动过程
|
||
while distance_moved < abs_distance * completion_threshold and 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_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
# 计算在移动方向上的位移
|
||
dx = current_position[0] - start_position[0]
|
||
dy = current_position[1] - start_position[1]
|
||
|
||
# 计算在初始方向上的投影距离（实际前进距离）
|
||
distance_moved = dx * direction_vector[0] + dy * direction_vector[1]
|
||
distance_moved = abs(distance_moved) # 确保距离为正值
|
||
|
||
# 计算垂直于移动方向的偏移量（y方向偏移）
|
||
y_offset = -dx * direction_vector[1] + dy * direction_vector[0]
|
||
|
||
# 累积y方向偏移量，检测持续偏移趋势
|
||
y_offset_accumulator = y_offset_accumulator * 0.8 + y_offset * 0.2
|
||
|
||
# 根据前进或后退确定期望方向
|
||
expected_direction = start_yaw if forward else (start_yaw + math.pi) % (2 * math.pi)
|
||
|
||
# 使用IMU朝向数据计算角度偏差
|
||
yaw_error = current_yaw - expected_direction
|
||
# 角度归一化
|
||
while yaw_error > math.pi:
|
||
yaw_error -= 2 * math.pi
|
||
while yaw_error < -math.pi:
|
||
yaw_error += 2 * math.pi
|
||
|
||
# 使用PID控制计算角速度修正
|
||
# 比例项
|
||
p_control = kp_angle * yaw_error
|
||
|
||
# 积分项 (带衰减)
|
||
angle_error_integral = angle_error_integral * 0.9 + yaw_error
|
||
angle_error_integral = max(-1.0, min(1.0, angle_error_integral)) # 限制积分范围
|
||
i_control = ki_angle * angle_error_integral
|
||
|
||
# 微分项
|
||
d_control = kd_angle * (yaw_error - last_angle_error)
|
||
last_angle_error = yaw_error
|
||
|
||
# 计算总的角速度控制量
|
||
angular_correction = -(p_control + i_control + d_control)
|
||
# 限制最大角速度修正
|
||
angular_correction = max(min(angular_correction, 0.3), -0.3)
|
||
|
||
# 根据持续的y偏移趋势增加侧向校正
|
||
lateral_correction = 0
|
||
if abs(y_offset_accumulator) > 0.05: # 如果累积偏移超过5厘米
|
||
lateral_correction = -y_offset_accumulator * 0.8 # 反向校正
|
||
lateral_correction = max(min(lateral_correction, 0.15), -0.15) # 限制最大侧向速度
|
||
|
||
if observe and abs(lateral_correction) > 0.05:
|
||
warning(f"累积Y偏移校正: {y_offset_accumulator:.3f}米，应用侧向速度 {lateral_correction:.3f}m/s", "偏移")
|
||
|
||
# 计算完成比例
|
||
completion_ratio = distance_moved / abs_distance
|
||
|
||
# 根据距离完成情况调整速度
|
||
if completion_ratio > slow_down_ratio:
|
||
# 计算减速系数
|
||
slow_factor = 1.0 - (completion_ratio - slow_down_ratio) / (1.0 - slow_down_ratio)
|
||
# 确保不会减速太多
|
||
slow_factor = max(0.3, slow_factor)
|
||
new_speed = actual_speed * slow_factor
|
||
|
||
if observe and abs(new_speed - msg.vel_des[0]) > 0.05:
|
||
info(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)", "移动")
|
||
|
||
actual_speed = new_speed
|
||
|
||
# 应用修正 - 同时应用角速度和侧向速度修正
|
||
msg.vel_des = [actual_speed, lateral_correction, angular_correction]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe and current_time % 1.0 < position_check_interval:
|
||
debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离")
|
||
debug(f"Y偏移: {y_offset:.3f}米, 累积偏移: {y_offset_accumulator:.3f}米", "偏移")
|
||
debug(f"朝向偏差: {math.degrees(yaw_error):.1f}度, 角速度修正: {angular_correction:.3f}rad/s", "角度")
|
||
debug(f"PID: P={p_control:.3f}, I={i_control:.3f}, D={d_control:.3f}", "控制")
|
||
|
||
# 更新检查时间
|
||
last_check_time = current_time
|
||
|
||
# 定期进行视觉检查和修正
|
||
if hasattr(ctrl, 'image_processor') and current_time - last_vision_check >= vision_check_interval:
|
||
try:
|
||
# 获取当前相机帧
|
||
frame = ctrl.image_processor.get_current_image()
|
||
if frame is not None:
|
||
# 检测轨道线 - 使用特定的石板路模式
|
||
center_info, _, _ = detect_dual_track_lines(
|
||
frame, observe=False, save_log=False)
|
||
|
||
# 如果成功检测到轨道线，使用它进行修正
|
||
if center_info is not None:
|
||
# 获取当前偏差
|
||
current_deviation = center_info["deviation"]
|
||
last_valid_deviation = current_deviation
|
||
|
||
# 添加到队列进行滤波
|
||
deviation_queue.append(current_deviation)
|
||
if len(deviation_queue) > filter_size:
|
||
deviation_queue.pop(0)
|
||
|
||
# 计算滤波后的偏差值 (去除最大和最小值后的平均)
|
||
if len(deviation_queue) >= 3:
|
||
filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue
|
||
filtered_deviation = sum(filtered_deviations) / len(filtered_deviations)
|
||
else:
|
||
filtered_deviation = current_deviation
|
||
|
||
# 计算视觉侧向修正速度
|
||
vision_lateral_correction = -filtered_deviation * vision_correction_gain
|
||
# 限制最大侧向速度修正
|
||
vision_lateral_correction = max(min(vision_lateral_correction, 0.2), -0.2)
|
||
|
||
# 与当前的侧向校正进行融合 (加权平均)
|
||
if msg.vel_des[1] != 0:
|
||
# 如果已经有侧向校正，与视觉校正进行融合
|
||
fused_lateral = msg.vel_des[1] * 0.3 + vision_lateral_correction * 0.7
|
||
else:
|
||
# 如果没有侧向校正，直接使用视觉校正
|
||
fused_lateral = vision_lateral_correction
|
||
|
||
if observe and abs(vision_lateral_correction) > 0.05:
|
||
warning(f"视觉修正: 偏差 {filtered_deviation:.1f}像素，应用侧向速度 {vision_lateral_correction:.3f}m/s", "视觉")
|
||
|
||
# 应用视觉修正，保留当前前进速度和角速度
|
||
msg.vel_des = [msg.vel_des[0], fused_lateral, msg.vel_des[2]]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe and current_time % 1.0 < vision_check_interval:
|
||
debug(f"视觉检测: 原始偏差 {current_deviation:.1f}像素, 滤波后 {filtered_deviation:.1f}像素", "视觉")
|
||
debug(f"融合侧向速度: {fused_lateral:.3f}m/s", "视觉")
|
||
except Exception as e:
|
||
if observe:
|
||
error(f"视觉检测异常: {e}", "错误")
|
||
|
||
# 更新视觉检查时间
|
||
last_vision_check = current_time
|
||
|
||
# 短暂延时
|
||
time.sleep(0.01)
|
||
|
||
# 平滑停止
|
||
slowdown_steps = 5
|
||
for i in range(slowdown_steps, 0, -1):
|
||
slowdown_factor = i / slowdown_steps
|
||
msg.vel_des = [actual_speed * slowdown_factor, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.1)
|
||
|
||
# 最后完全停止
|
||
if hasattr(ctrl.base_msg, 'stop_smooth'):
|
||
ctrl.base_msg.stop_smooth()
|
||
else:
|
||
ctrl.base_msg.stop()
|
||
|
||
# 获取最终位置和实际移动距离
|
||
final_position = ctrl.odo_msg.xyz
|
||
dx = final_position[0] - start_position[0]
|
||
dy = final_position[1] - start_position[1]
|
||
actual_distance = math.sqrt(dx*dx + dy*dy)
|
||
|
||
# 计算最终y方向偏移
|
||
final_y_offset = -dx * direction_vector[1] + dy * direction_vector[0]
|
||
|
||
if observe:
|
||
success(f"石板路增强直线移动完成，实际移动距离: {actual_distance:.3f}米", "完成")
|
||
info(f"最终Y方向偏移: {final_y_offset:.3f}米", "偏移")
|
||
|
||
# 判断是否成功完成
|
||
distance_error = abs(actual_distance - abs_distance)
|
||
y_offset_error = abs(final_y_offset)
|
||
|
||
go_success = distance_error < 0.1 and y_offset_error < 0.1 # 距离误差和y偏移都小于10厘米视为成功
|
||
|
||
if observe:
|
||
if go_success:
|
||
success(f"移动成功", "成功")
|
||
else:
|
||
warning(f"移动不够精确，距离误差: {distance_error:.3f}米, Y偏移: {y_offset_error:.3f}米", "警告")
|
||
|
||
return go_success
|