Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'task-3' of ssh://120.27.199.238:222/Havoc420mac/mi-task into task-3

Browse Source
This commit is contained in:
Havoc 2025-05-28 18:08:34 +08:00
commit 5502fa55d3
2 changed files with 309 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
task_3/task_3.py
View File

@ -6,6 +6,8 @@ import copy
import math
import lcm
import numpy as np
import cv2
import tempfile
# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
@ -17,6 +19,7 @@ from base_move.turn_degree import turn_degree, turn_degree_v2
from base_move.go_straight import go_straight
from base_move.go_to_xy import go_to_x_v2
from file_send_lcmt import file_send_lcmt
from utils.yellow_area_analyzer import analyze_yellow_area_ratio
# 创建本模块特定的日志记录器
logger = get_logger("任务3")
@ -35,6 +38,7 @@ robot_cmd = {
'value':0, 'duration':0
}
def pass_up_down(ctrl, msg):
usergait_msg = file_send_lcmt()
lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
@ -332,6 +336,164 @@ def pass_up_down(ctrl, msg):
pass
def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_time=30, observe=True):
"""
控制机器人直走，直到摄像头检测到黄色区域比例超过指定阈值后开始下降时停止
参数:
ctrl: Robot_Ctrl 对象，包含里程计信息
msg: robot_control_cmd_lcmt 对象，用于发送命令
yellow_ratio_threshold: 黄色区域占比阈值(0-1之间的浮点数)，默认为0.15
speed: 前进速度(米/秒)，默认为0.3米/秒
max_time: 最大行走时间(秒)，默认为30秒
observe: 是否输出中间状态信息和可视化结果，默认为True
返回:
bool: 是否成功检测到黄色区域并停止
"""
section("开始直行寻找黄色区域", "黄色检测")
# 设置移动命令基本参数
msg.mode = 11 # Locomotion模式
msg.gait_id = 26 # 自变频步态
msg.duration = 0 # wait next cmd
msg.step_height = [0.06, 0.06] # 抬腿高度
msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度]
# 记录起始时间和位置
start_time = time.time()
start_position = list(ctrl.odo_msg.xyz)
if observe:
info(f"开始寻找黄色区域，阈值: {yellow_ratio_threshold:.2%}", "启动")
debug(f"起始位置: {start_position}", "位置")
# 检测间隔
check_interval = 0.3 # 秒
last_check_time = 0
# 黄色区域监测变量
yellow_peak_detected = False # 是否检测到峰值
yellow_decreasing = False # 是否开始下降
max_yellow_ratio = 0.0 # 记录最大黄色区域占比
yellow_ratio_history = [] # 记录黄色区域占比历史
history_window_size = 5 # 历史窗口大小，用于平滑处理
try:
# 开始移动
msg.life_count += 1
ctrl.Send_cmd(msg)
# 持续检测黄色区域
while time.time() - start_time < max_time and not yellow_decreasing:
current_time = time.time()
# 定期发送移动命令保持移动状态
if current_time - last_check_time >= check_interval:
# 获取当前图像并保存到临时文件
current_image = ctrl.image_processor.get_current_image()
# 创建临时文件保存图像
with tempfile.NamedTemporaryFile(suffix='.jpg', delete=False) as temp_file:
temp_filename = temp_file.name
cv2.imwrite(temp_filename, current_image)
try:
# 分析图像中的黄色区域
yellow_ratio = analyze_yellow_area_ratio(temp_filename, debug=False, save_result=False)
# 添加到历史记录
yellow_ratio_history.append(yellow_ratio)
if len(yellow_ratio_history) > history_window_size:
yellow_ratio_history.pop(0)
# 计算平滑后的当前黄色占比（使用最近几次的平均值以减少噪声）
current_smooth_ratio = sum(yellow_ratio_history) / len(yellow_ratio_history)
# 计算已移动距离（仅用于显示）
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:
info(f"当前黄色区域占比: {yellow_ratio:.2%}, 平滑值: {current_smooth_ratio:.2%}, 已移动: {distance_moved:.2f}米", "检测")
# 检测是否达到阈值（开始监测峰值）
if current_smooth_ratio >= yellow_ratio_threshold:
# 更新最大值
if current_smooth_ratio > max_yellow_ratio:
max_yellow_ratio = current_smooth_ratio
if not yellow_peak_detected:
yellow_peak_detected = True
if observe:
info(f"黄色区域占比超过阈值，开始监测峰值", "检测")
# 检测是否开始下降
elif yellow_peak_detected and current_smooth_ratio < max_yellow_ratio * 0.9: # 下降到峰值的90%以下认为开始下降
yellow_decreasing = True
if observe:
success(f"检测到黄色区域占比开始下降，峰值: {max_yellow_ratio:.2%}, 当前: {current_smooth_ratio:.2%}", "检测")
finally:
# 删除临时文件
try:
os.unlink(temp_filename)
except:
pass
# 更新心跳
msg.life_count += 1
ctrl.Send_cmd(msg)
last_check_time = current_time
# 小间隔
time.sleep(0.05)
# 平滑停止
if yellow_decreasing:
if observe:
info("开始平滑停止", "停止")
# 先降低速度再停止，实现平滑停止
slowdown_steps = 5
for i in range(slowdown_steps, 0, -1):
slowdown_factor = i / slowdown_steps
msg.vel_des = [speed * slowdown_factor, 0, 0]
msg.life_count += 1
ctrl.Send_cmd(msg)
time.sleep(0.1)
# 完全停止
ctrl.base_msg.stop()
# 计算最终移动距离
final_position = ctrl.odo_msg.xyz
dx = final_position[0] - start_position[0]
dy = final_position[1] - start_position[1]
final_distance = math.sqrt(dx*dx + dy*dy)
if observe:
if yellow_decreasing:
success(f"成功检测到黄色区域峰值并停止，峰值占比: {max_yellow_ratio:.2%}, 总移动距离: {final_distance:.2f}米", "完成")
else:
warning(f"未能在限定时间内检测到黄色区域峰值，总移动距离: {final_distance:.2f}米", "超时")
return yellow_decreasing
except KeyboardInterrupt:
# 处理键盘中断
ctrl.base_msg.stop()
if observe:
warning("操作被用户中断", "中断")
return False
except Exception as e:
# 处理其他异常
ctrl.base_msg.stop()
if observe:
error(f"发生错误: {str(e)}", "错误")
return False
def run_task_3(ctrl, msg):
section('任务3：步态切换', "启动")
info('开始执行任务3...', "启动")
@ -342,4 +504,19 @@ def run_task_3(ctrl, msg):
# pass_up_down(ctrl, msg)
section('任务3-2：yellow stop', "开始")
go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
# 原地站立3秒
section("原地站立3秒", "站立")
msg.mode = 11 # Locomotion模式
msg.gait_id = 26 # 自变频步态
msg.duration = 0 # wait next cmd
msg.step_height = [0.06, 0.06] # 抬腿高度
msg.vel_des = [0, 0, 0] # 零速度，原地站立
msg.life_count += 1
ctrl.Send_cmd(msg)
info("开始原地站立3秒", "站立")
time.sleep(3)
info("完成原地站立", "站立")

132
utils/yellow_area_analyzer.py Normal file
View File

@ -0,0 +1,132 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import os
import argparse
import matplotlib.pyplot as plt
def analyze_yellow_area_ratio(image_path, debug=False, save_result=False):
"""
专门针对黄色区域的分析算法
参数:
image_path: 图片路径
debug: 是否显示处理过程中的图像，用于调试
save_result: 是否保存处理结果图像
返回:
yellow_ratio: 黄色区域占比（0-1之间的浮点数）
"""
# 读取图片
img = cv2.imread(image_path)
if img is None:
raise ValueError(f"无法读取图片: {image_path}")
# 获取图片文件名（不带路径和扩展名）
filename = os.path.splitext(os.path.basename(image_path))[0]
# 转换为HSV色彩空间（更适合颜色分割）
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# 提取图像的各个通道
h, s, v = cv2.split(hsv)
# 黄色在HSV中的范围：色调约为20-30度（OpenCV中为10-30）
# 黄色通常有较高的饱和度和亮度
yellow_hue_lower = np.array([20, 100, 100])
yellow_hue_upper = np.array([40, 255, 255])
# 创建黄色区域掩码
yellow_mask = cv2.inRange(hsv, yellow_hue_lower, yellow_hue_upper)
# 应用形态学操作
kernel = np.ones((5, 5), np.uint8)
yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_OPEN, kernel)
yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_CLOSE, kernel)
# 使用连通区域分析，去除小的噪点区域
num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(yellow_mask, connectivity=8)
# 过滤小的连通区域
min_size = 500 # 最小连通区域大小
filtered_yellow_mask = np.zeros_like(yellow_mask)
# 从索引1开始，因为0是背景
for i in range(1, num_labels):
if stats[i, cv2.CC_STAT_AREA] >= min_size:
filtered_yellow_mask[labels == i] = 255
# 计算黄色区域占比
height, width = yellow_mask.shape
total_pixels = height * width
yellow_pixels = np.sum(filtered_yellow_mask == 255)
yellow_ratio = yellow_pixels / total_pixels
# 在原图上标记黄色区域
result = img.copy()
overlay = img.copy()
overlay[filtered_yellow_mask > 0] = [0, 165, 255] # 用橙色标记黄色区域
cv2.addWeighted(overlay, 0.4, img, 0.6, 0, result) # 半透明效果
# 显示检测结果信息
cv2.putText(result, f"Yellow Ratio: {yellow_ratio:.2%}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 165, 255), 2)
# 调试模式：显示处理过程图像
if debug:
plt.figure(figsize=(15, 10))
plt.subplot(231)
plt.title("Original Image")
plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
plt.subplot(232)
plt.title("Hue Channel")
plt.imshow(h, cmap='hsv')
plt.subplot(233)
plt.title("Saturation Channel")
plt.imshow(s, cmap='gray')
plt.subplot(234)
plt.title("Initial Yellow Mask")
plt.imshow(yellow_mask, cmap='gray')
plt.subplot(235)
plt.title("Filtered Yellow Mask")
plt.imshow(filtered_yellow_mask, cmap='gray')
plt.subplot(236)
plt.title("Yellow Detection Result")
plt.imshow(cv2.cvtColor(result, cv2.COLOR_BGR2RGB))
plt.tight_layout()
plt.show()
# 保存结果
if save_result:
result_dir = "results"
os.makedirs(result_dir, exist_ok=True)
output_path = os.path.join(result_dir, f"{filename}_yellow_area_result.jpg")
cv2.imwrite(output_path, result)
print(f"结果已保存至: {output_path}")
return yellow_ratio
def main():
parser = argparse.ArgumentParser(description='分析图片中黄色区域占比')
parser.add_argument('--image_path', default='./image_20250525_090252.png', type=str, help='图片路径')
parser.add_argument('--debug', default=False, action='store_true', help='显示处理过程图像')
parser.add_argument('--save', action='store_true', help='保存处理结果图像')
args = parser.parse_args()
try:
yellow_ratio = analyze_yellow_area_ratio(args.image_path, args.debug, args.save)
print(f"黄色区域占比: {yellow_ratio:.2%}")
except Exception as e:
print(f"错误: {e}")
if __name__ == "__main__":
main()