Havoc
6c55201f73
- 将任务5中的run_task_5函数重命名为pass_bar，并调整其实现以适应新的步态参数文件路径。 - 在task_4.py中添加通过栏杆的步骤，并更新相关的步态参数文件路径。 - 优化了任务4的执行流程，确保机器人能够顺利通过栏杆并继续后续任务。
145 lines
5.0 KiB
Python
145 lines
5.0 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
|
||
from base_move.go_straight import go_straight
|
||
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 task_4.pass_bar import pass_bar
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务4")
|
||
|
||
def run_task_4(ctrl, msg):
|
||
"""
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: 控制消息对象
|
||
image_processor: 可选的图像处理器实例
|
||
"""
|
||
section('任务4-1：移动直到灰色天空比例小于阈值', "天空检测")
|
||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)
|
||
|
||
section('任务4-2：通过栏杆', "移动")
|
||
pass_bar(ctrl, msg)
|
||
|
||
section('任务4-3：直线移动 - 石板路', "移动")
|
||
# 设置机器人运动模式为快步跑
|
||
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(5000) # 持续5秒钟
|
||
|
||
|
||
def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=10, 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 run_task_4_back(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(5000) # 持续5秒钟
|
||
|
||
section('任务4-2：移动直到灰色天空比例小于阈值', "天空检测")
|
||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)
|
||
|
||
section('任务4-3：通过栏杆', "移动")
|
||
pass_bar(ctrl, msg)
|
||
|
||
|