188 lines
7.3 KiB
Python
Executable File
188 lines
7.3 KiB
Python
Executable File
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
|
||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||
from task_3.task_3 import pass_stone
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务4")
|
||
|
||
observe = True
|
||
|
||
STONE_DISTANCE = 4.0 # TODO 距离参数需要微调
|
||
RED_RATIO_THRESHOLD = 0.35 # TODO 红色区域比例阈值需要微调
|
||
|
||
def run_task_4(ctrl, msg):
|
||
info('开始执行任务4...', "启动")
|
||
turn_degree_v2(ctrl, msg, 90, absolute=True)#wzl:8.20新增，右转90度
|
||
|
||
section('任务4-1：直线移动', "移动")
|
||
pass_stone(ctrl, msg, distance=STONE_DISTANCE)
|
||
|
||
section('任务4-2：移动直到红色区域比例大于阈值', "红色检测")
|
||
go_straight_until_red_bar(ctrl, msg, red_ratio_threshold=RED_RATIO_THRESHOLD, speed=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_deviation=10.0, observe=False)
|
||
|
||
go_straight(ctrl, msg, distance=2, speed=1, observe=True)
|
||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
|
||
|
||
# TODO 向右移动0.5秒 (或许不需要了)
|
||
# section('任务4-回程：向右移动', "移动")
|
||
# go_lateral(ctrl, msg, distance=-0.1, speed=0.15, observe=True) # DEBUG
|
||
|
||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||
|
||
section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
|
||
go_straight_until_red_bar(ctrl, msg, red_ratio_threshold=RED_RATIO_THRESHOLD, speed=0.2)
|
||
|
||
section('任务4-2：通过栏杆', "移动")
|
||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||
pass_bar(ctrl, msg)
|
||
|
||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||
section('任务4-3：stone', "移动")
|
||
go_straight(ctrl, msg, distance=1, speed=2)
|
||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||
|
||
# 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)
|
||
pass_stone(ctrl, msg, distance=STONE_DISTANCE)
|
||
# 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_red_bar(ctrl, msg,
|
||
red_ratio_threshold=0.2,
|
||
step_distance=0.5,
|
||
max_distance=5,
|
||
speed=0.3
|
||
):
|
||
"""
|
||
控制机器人沿直线行走，直到红色区域比例高于指定阈值
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制命令消息对象
|
||
red_ratio_threshold: 红色区域比例阈值，当检测到的比例高于此值时停止
|
||
step_distance: 每次移动的步长(米)
|
||
max_distance: 最大移动距离(米)，防止无限前进
|
||
speed: 移动速度(米/秒)
|
||
|
||
返回:
|
||
bool: 是否成功找到红色区域比例高于阈值的位置
|
||
"""
|
||
def analyze_red_area_ratio(image):
|
||
"""
|
||
分析图像中红色区域的占比
|
||
输入: BGR图像
|
||
返回: 红色区域占比 (0~1)
|
||
"""
|
||
# 转换到HSV空间
|
||
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
|
||
# 红色有两个区间
|
||
lower_red1 = np.array([0, 70, 50])
|
||
upper_red1 = np.array([10, 255, 255])
|
||
lower_red2 = np.array([160, 70, 50])
|
||
upper_red2 = np.array([180, 255, 255])
|
||
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
|
||
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
|
||
mask = cv2.bitwise_or(mask1, mask2)
|
||
red_pixels = np.count_nonzero(mask)
|
||
total_pixels = mask.shape[0] * mask.shape[1]
|
||
ratio = red_pixels / total_pixels if total_pixels > 0 else 0
|
||
return ratio
|
||
|
||
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('ai')
|
||
if current_image is None:
|
||
warning("无法获取图像，等待...", "图像")
|
||
time.sleep(0.5)
|
||
continue
|
||
|
||
# 分析红色区域比例
|
||
try:
|
||
red_ratio = analyze_red_area_ratio(current_image)
|
||
info(f"当前红色区域比例: {red_ratio:.2%}", "分析")
|
||
|
||
# 如果红色区域比例高于阈值，停止移动
|
||
if red_ratio > red_ratio_threshold:
|
||
success(f"检测到红色区域比例({red_ratio:.2%})高于阈值({red_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} 米，但未找到红色区域比例高于 {red_ratio_threshold:.2%} 的位置", "超时")
|
||
|
||
return success_flag
|