173 lines
6.4 KiB
Python
173 lines
6.4 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
import cv2
|
||
import numpy as np
|
||
|
||
# 添加父目录到路径，以便能够导入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 base_move.move_base_hori_line import go_straight_until_hori_line
|
||
from base_move.follow_dual_tracks import go_straight_with_visual_track
|
||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||
|
||
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)
|
||
|
||
section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
|
||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5)
|
||
|
||
section('任务4-2：通过栏杆', "移动")
|
||
pass_bar(ctrl, msg)
|
||
|
||
section('任务4-3：stone', "移动")
|
||
go_straight(ctrl, msg, distance=1, speed=2)
|
||
# go_straight_with_visual_track(ctrl, msg, distance=4.5, observe=False,
|
||
# mode=11,
|
||
# speed=0.35,
|
||
# gait_id=3,
|
||
# step_height=[0.21, 0.21],
|
||
# )
|
||
go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||
|
||
section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
|
||
# 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
|
||
go_straight_until_hori_line(
|
||
ctrl,
|
||
msg,
|
||
target_distance=1, # 目标与黄线的距离(米)
|
||
max_distance=6.0, # 最大搜索距离(米)
|
||
speed=0.2, # 移动速度(米/秒)
|
||
mode=11, # 运动模式
|
||
gait_id=3, # 步态ID（快步跑）
|
||
step_height=[0.21, 0.21], # 摆动腿离地高度
|
||
observe=True # 显示调试信息
|
||
)
|
||
|
||
go_straight(ctrl, msg, distance=0.8, speed=0.2, observe=True, 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
|