269 lines
9.8 KiB
Python
269 lines
9.8 KiB
Python
|
import time
|
|||
|
|
import threading
|
|||
|
|
import re
|
|||
|
|
import numpy as np
|
|||
|
|
import sys
|
|||
|
|
import os
|
|||
|
|
import signal
|
|||
|
|
from openai import OpenAI
|
|||
|
|
from picamera2 import Picamera2
|
|||
|
|
import io
|
|||
|
|
from PIL import Image
|
|||
|
|
import base64
|
|||
|
|
|
|||
|
|
# 配置项目路径（根据你的实际路径修改）
|
|||
|
|
PROJECT_ROOT = "/home/duckpi/open_duck_mini_ws/OPEN_DUCK_MINI/Open_Duck_Mini_Runtime-2"
|
|||
|
|
ONNX_MODEL_PATH = "/home/duckpi/open_duck_mini_ws/OPEN_DUCK_MINI/Open_Duck_Mini-2/BEST_WALK_ONNX_2.onnx"
|
|||
|
|
sys.path.append(PROJECT_ROOT)
|
|||
|
|
|
|||
|
|
# 导入运动控制模块
|
|||
|
|
from v2_rl_walk_mujoco import RLWalk
|
|||
|
|
|
|||
|
|
# API配置（替换为你的实际密钥）
|
|||
|
|
ARK_API_KEY = "390d517c-129a-41c1-bf3d-458048007b69"
|
|||
|
|
ARK_MODEL_ID = "doubao-seed-1-6-250615"
|
|||
|
|
|
|||
|
|
class SimpleTTS:
|
|||
|
|
"""简化的TTS模块，仅用于测试反馈"""
|
|||
|
|
def speak(self, text):
|
|||
|
|
print(f"[语音反馈] {text}")
|
|||
|
|
|
|||
|
|
class MotionController:
|
|||
|
|
"""运动控制封装"""
|
|||
|
|
def __init__(self):
|
|||
|
|
try:
|
|||
|
|
self.rl_walk = RLWalk(
|
|||
|
|
onnx_model_path=ONNX_MODEL_PATH,
|
|||
|
|
cutoff_frequency=40,
|
|||
|
|
pid=[30, 0, 0]
|
|||
|
|
)
|
|||
|
|
self.walk_thread = threading.Thread(target=self.rl_walk.run, daemon=True)
|
|||
|
|
self.walk_thread.start()
|
|||
|
|
time.sleep(1)
|
|||
|
|
print("✅ 运动控制模块初始化成功")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 运动控制初始化失败：{str(e)}")
|
|||
|
|
sys.exit(1)
|
|||
|
|
|
|||
|
|
def execute_motion(self, action_name: str, seconds: float):
|
|||
|
|
"""执行指定动作"""
|
|||
|
|
try:
|
|||
|
|
if action_name == "move_forward":
|
|||
|
|
print(f"🚶 前进{seconds}秒...")
|
|||
|
|
self.rl_walk.last_commands[0] = 0.17
|
|||
|
|
elif action_name == "move_backward":
|
|||
|
|
print(f"🚶 后退{seconds}秒...")
|
|||
|
|
self.rl_walk.last_commands[0] = -0.17
|
|||
|
|
elif action_name == "turn_left":
|
|||
|
|
print(f"🔄 左转{seconds}秒...")
|
|||
|
|
self.rl_walk.last_commands[2] = 1.1
|
|||
|
|
elif action_name == "turn_right":
|
|||
|
|
print(f"🔄 右转{seconds}秒...")
|
|||
|
|
self.rl_walk.last_commands[2] = -1.1
|
|||
|
|
|
|||
|
|
time.sleep(seconds)
|
|||
|
|
self.rl_walk.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
|||
|
|
print("✅ 动作完成")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 动作执行失败：{str(e)}")
|
|||
|
|
self.rl_walk.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
|||
|
|
|
|||
|
|
class CameraModule:
|
|||
|
|
"""摄像头模块封装"""
|
|||
|
|
def __init__(self):
|
|||
|
|
try:
|
|||
|
|
self.camera = Picamera2()
|
|||
|
|
cam_config = self.camera.create_still_configuration(main={"size": (320, 240)})
|
|||
|
|
self.camera.configure(cam_config)
|
|||
|
|
self.camera.start()
|
|||
|
|
print("✅ 摄像头模块初始化成功")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 摄像头初始化失败：{str(e)}")
|
|||
|
|
sys.exit(1)
|
|||
|
|
|
|||
|
|
def capture_base64(self):
|
|||
|
|
"""拍摄并返回base64编码图像"""
|
|||
|
|
try:
|
|||
|
|
img_array = self.camera.capture_array()
|
|||
|
|
img_byte = io.BytesIO()
|
|||
|
|
Image.fromarray(img_array).save(img_byte, format="JPEG", quality=80)
|
|||
|
|
return base64.b64encode(img_byte.getvalue()).decode("utf-8")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 拍摄失败：{str(e)}")
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
class ObjectTracker:
|
|||
|
|
"""物品追踪核心类"""
|
|||
|
|
def __init__(self, target_name="万用表", tts=None, motion=None, camera=None):
|
|||
|
|
self.target_name = target_name
|
|||
|
|
self.tracking_active = False
|
|||
|
|
self.target_lost_count = 0
|
|||
|
|
self.max_lost_count = 5 # 连续丢失次数阈值
|
|||
|
|
self.tts = tts or SimpleTTS()
|
|||
|
|
self.motion = motion or MotionController()
|
|||
|
|
self.camera = camera or CameraModule()
|
|||
|
|
self.client = OpenAI(
|
|||
|
|
base_url="https://ark.cn-beijing.volces.com/api/v3",
|
|||
|
|
api_key=ARK_API_KEY
|
|||
|
|
)
|
|||
|
|
print(f"✅ 追踪器初始化完成，目标：{self.target_name}")
|
|||
|
|
|
|||
|
|
def get_object_position(self, image_base64):
|
|||
|
|
"""获取目标在图像中的位置信息"""
|
|||
|
|
try:
|
|||
|
|
response = self.client.chat.completions.create(
|
|||
|
|
model=ARK_MODEL_ID,
|
|||
|
|
messages=[{
|
|||
|
|
"role": "user",
|
|||
|
|
"content": [
|
|||
|
|
{"type": "image_url", "image_url": f"data:image/jpeg;base64,{image_base64}"},
|
|||
|
|
{"type": "text", "text": (f"请识别图像中的'{self.target_name}'，返回其位置信息。"
|
|||
|
|
"格式要求：中心X坐标(0-100)，中心Y坐标(0-100)，宽度占比(0-100)，高度占比(0-100)。"
|
|||
|
|
"如果未找到，返回'未找到'")}
|
|||
|
|
]
|
|||
|
|
}]
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
content = response.choices[0].message.content
|
|||
|
|
if "未找到" in content:
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
# 提取数字信息
|
|||
|
|
nums = re.findall(r"\d+\.?\d*", content)
|
|||
|
|
if len(nums) >= 4:
|
|||
|
|
return {
|
|||
|
|
"center_x": float(nums[0]),
|
|||
|
|
"center_y": float(nums[1]),
|
|||
|
|
"width": float(nums[2]),
|
|||
|
|
"height": float(nums[3])
|
|||
|
|
}
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 目标识别出错: {str(e)}")
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
def track_object(self):
|
|||
|
|
"""追踪主循环"""
|
|||
|
|
self.tts.speak(f"开始追踪{self.target_name}")
|
|||
|
|
self.tracking_active = True
|
|||
|
|
self.target_lost_count = 0
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
while self.tracking_active:
|
|||
|
|
# 1. 采集图像
|
|||
|
|
image_base64 = self.camera.capture_base64()
|
|||
|
|
if not image_base64:
|
|||
|
|
time.sleep(1)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
# 2. 识别目标位置
|
|||
|
|
pos = self.get_object_position(image_base64)
|
|||
|
|
if not pos:
|
|||
|
|
self.target_lost_count += 1
|
|||
|
|
print(f"⚠️ 未找到{self.target_name}，已连续丢失{self.target_lost_count}次")
|
|||
|
|
|
|||
|
|
if self.target_lost_count >= self.max_lost_count:
|
|||
|
|
self.tts.speak(f"已丢失{self.target_name}，停止追踪")
|
|||
|
|
self.stop_tracking()
|
|||
|
|
time.sleep(1)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
# 重置丢失计数
|
|||
|
|
self.target_lost_count = 0
|
|||
|
|
print(f"🎯 发现{self.target_name} - 中心X: {pos['center_x']}, 宽度占比: {pos['width']}")
|
|||
|
|
|
|||
|
|
# 3. 根据位置控制移动
|
|||
|
|
self.control_movement(pos)
|
|||
|
|
time.sleep(1.5) # 控制追踪频率
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"❌ 追踪过程出错: {str(e)}")
|
|||
|
|
self.stop_tracking()
|
|||
|
|
|
|||
|
|
def control_movement(self, pos):
|
|||
|
|
"""根据目标位置控制移动"""
|
|||
|
|
# 横向位置控制 (左右转向)
|
|||
|
|
if pos["center_x"] < 35: # 目标偏左
|
|||
|
|
self.tts.speak("目标在左边，向左转")
|
|||
|
|
self.motion.execute_motion("turn_left", 0.8)
|
|||
|
|
elif pos["center_x"] > 65: # 目标偏右
|
|||
|
|
self.tts.speak("目标在右边，向右转")
|
|||
|
|
self.motion.execute_motion("turn_right", 0.8)
|
|||
|
|
|
|||
|
|
# 距离控制 (前进后退)
|
|||
|
|
if pos["width"] < 20: # 目标过小，距离过远
|
|||
|
|
self.tts.speak("距离目标较远，前进")
|
|||
|
|
self.motion.execute_motion("move_forward", 1.5)
|
|||
|
|
elif pos["width"] > 40: # 目标过大，距离过近
|
|||
|
|
self.tts.speak("距离目标过近，后退")
|
|||
|
|
self.motion.execute_motion("move_backward", 1)
|
|||
|
|
else:
|
|||
|
|
self.tts.speak("已对准目标，保持位置")
|
|||
|
|
|
|||
|
|
def start_tracking(self):
|
|||
|
|
"""启动追踪线程"""
|
|||
|
|
if not self.tracking_active:
|
|||
|
|
threading.Thread(target=self.track_object, daemon=True).start()
|
|||
|
|
|
|||
|
|
def stop_tracking(self):
|
|||
|
|
"""停止追踪"""
|
|||
|
|
self.tracking_active = False
|
|||
|
|
print(f"🛑 停止追踪{self.target_name}")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def main():
|
|||
|
|
# 初始化组件
|
|||
|
|
tts = SimpleTTS()
|
|||
|
|
motion = MotionController()
|
|||
|
|
camera = CameraModule()
|
|||
|
|
tracker = ObjectTracker(
|
|||
|
|
target_name="万用表", # 可修改为其他目标，如"水杯"、"书本"
|
|||
|
|
tts=tts,
|
|||
|
|
motion=motion,
|
|||
|
|
camera=camera
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 信号处理（优雅退出）
|
|||
|
|
def handle_interrupt(signum, frame):
|
|||
|
|
print("\n🛑 收到退出信号，正在停止...")
|
|||
|
|
tracker.stop_tracking()
|
|||
|
|
motion.rl_walk.last_commands = [0.0, 0.0, 0.0] # 停止运动
|
|||
|
|
camera.camera.stop() # 关闭摄像头
|
|||
|
|
print("✅ 所有资源已释放，程序退出")
|
|||
|
|
sys.exit(0)
|
|||
|
|
|
|||
|
|
signal.signal(signal.SIGINT, handle_interrupt)
|
|||
|
|
|
|||
|
|
# 交互提示
|
|||
|
|
print("\n===== 物品追踪测试程序 =====")
|
|||
|
|
print("操作说明:")
|
|||
|
|
print(" s - 开始追踪目标")
|
|||
|
|
print(" t - 停止追踪")
|
|||
|
|
print(" q - 退出程序")
|
|||
|
|
print("===========================")
|
|||
|
|
|
|||
|
|
# 主交互循环
|
|||
|
|
while True:
|
|||
|
|
cmd = input("请输入指令: ").strip().lower()
|
|||
|
|
if cmd == 's':
|
|||
|
|
if not tracker.tracking_active:
|
|||
|
|
print("▶️ 开始追踪...")
|
|||
|
|
tracker.start_tracking()
|
|||
|
|
else:
|
|||
|
|
print("⚠️ 已经在追踪中")
|
|||
|
|
elif cmd == 't':
|
|||
|
|
if tracker.tracking_active:
|
|||
|
|
tracker.stop_tracking()
|
|||
|
|
print("⏹️ 已停止追踪")
|
|||
|
|
else:
|
|||
|
|
print("⚠️ 没有正在进行的追踪")
|
|||
|
|
elif cmd == 'q':
|
|||
|
|
handle_interrupt(None, None)
|
|||
|
|
else:
|
|||
|
|
print("❓ 未知指令，请输入 s/t/q")
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
|
|||
|
|
main()
|