75 lines
2.5 KiB
Python
75 lines
2.5 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
|
||
from task_5.detect_arrow_direction import ArrowDetector
|
||
|
||
def run_task_5(ctrl, msg, image_processor=None):
|
||
# 初始化箭头检测器
|
||
detector = ArrowDetector() if image_processor is None else ArrowDetector(image_processor)
|
||
|
||
try:
|
||
# 设置俯身姿态
|
||
msg.mode = 3 # 姿态控制模式
|
||
msg.gait_id = 0
|
||
msg.pos_des = [0, 0, 0.15] # 设置较低的姿态高度
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
ctrl.Wait_finish(3, msg.gait_id)
|
||
|
||
# 等待姿态稳定
|
||
time.sleep(1.0)
|
||
|
||
# 等待获取图像和检测箭头方向
|
||
print("正在检测箭头方向...")
|
||
time.sleep(2.0) # 给检测器一些时间来获取图像
|
||
|
||
# 检测箭头方向
|
||
direction = detector.get_arrow_direction()
|
||
print(f"检测到的箭头方向: {direction}")
|
||
|
||
# 保存检测结果的可视化图像
|
||
detector.visualize_current_detection("arrow_detection_result.jpg")
|
||
|
||
# 根据箭头方向决定移动方向
|
||
vel_x = 0.5 # 前进速度
|
||
vel_y = 0.0 # 初始侧向速度为0
|
||
|
||
if direction == "left":
|
||
# 如果是左箭头，向左移动
|
||
vel_y = 0.3 # 向左移动的速度
|
||
print("根据箭头方向，向左移动")
|
||
elif direction == "right":
|
||
# 如果是右箭头，向右移动
|
||
vel_y = -0.3 # 向右移动的速度
|
||
print("根据箭头方向，向右移动")
|
||
else:
|
||
# 如果无法确定方向，直接前进
|
||
print("无法确定箭头方向，直接前进")
|
||
|
||
# 开始运动
|
||
msg.mode = 11 # 运动控制模式
|
||
msg.gait_id = 3 # 使用 trot 步态
|
||
msg.vel_des = [vel_x, vel_y, 0] # 设置移动速度，x和y方向
|
||
msg.duration = 3000 # 运动持续3秒
|
||
msg.step_height = [0.03, 0.03] # 设置步高
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
ctrl.Wait_finish(11, msg.gait_id)
|
||
|
||
# 恢复站立姿态
|
||
msg.mode = 3
|
||
msg.gait_id = 0
|
||
msg.pos_des = [0, 0, 0.25] # 恢复到正常站立高度
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
ctrl.Wait_finish(3, msg.gait_id)
|
||
|
||
finally:
|
||
# 清理资源
|
||
detector.destroy()
|
||
|