feat: implement new animation iface
This commit is contained in:
Eli Yip
parent
44dff010d8
commit
5491756a0c
12
pkg/device/animation.go
Normal file
12
pkg/device/animation.go
Normal file
@ -0,0 +1,12 @@
|
||||
package device
|
||||
|
||||
// Animation 定义了一个动画序列的行为
|
||||
type Animation interface {
|
||||
// Run 执行动画的一个周期或直到被停止
|
||||
// executor: 用于执行姿态指令
|
||||
// stop: 接收停止信号的通道
|
||||
// speedMs: 动画执行的速度（毫秒）
|
||||
Run(executor PoseExecutor, stop <-chan struct{}, speedMs int) error
|
||||
// Name 返回动画的名称
|
||||
Name() string
|
||||
}
|
||||
@ -17,6 +17,10 @@ type Device interface {
|
||||
GetStatus() (DeviceStatus, error) // 获取设备状态
|
||||
Connect() error // 连接设备
|
||||
Disconnect() error // 断开设备连接
|
||||
|
||||
// --- 新增 ---
|
||||
PoseExecutor // 嵌入 PoseExecutor 接口，Device 需实现它
|
||||
GetAnimationEngine() *AnimationEngine // 获取设备的动画引擎
|
||||
}
|
||||
|
||||
// Command 代表一个发送给设备的指令
|
||||
|
||||
191
pkg/device/engine.go
Normal file
191
pkg/device/engine.go
Normal file
@ -0,0 +1,191 @@
|
||||
package device
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"sync"
|
||||
)
|
||||
|
||||
// defaultAnimationSpeedMs 定义默认动画速度（毫秒）
|
||||
const defaultAnimationSpeedMs = 500
|
||||
|
||||
// AnimationEngine 管理和执行动画
|
||||
type AnimationEngine struct {
|
||||
executor PoseExecutor // 关联的姿态执行器
|
||||
animations map[string]Animation // 注册的动画
|
||||
stopChan chan struct{} // 当前动画的停止通道
|
||||
current string // 当前运行的动画名称
|
||||
isRunning bool // 是否有动画在运行
|
||||
engineMutex sync.Mutex // 保护引擎状态 (isRunning, current, stopChan)
|
||||
registerMutex sync.RWMutex // 保护动画注册表 (animations)
|
||||
}
|
||||
|
||||
// NewAnimationEngine 创建一个新的动画引擎
|
||||
func NewAnimationEngine(executor PoseExecutor) *AnimationEngine {
|
||||
return &AnimationEngine{
|
||||
executor: executor,
|
||||
animations: make(map[string]Animation),
|
||||
}
|
||||
}
|
||||
|
||||
// Register 注册一个动画
|
||||
func (e *AnimationEngine) Register(anim Animation) {
|
||||
e.registerMutex.Lock()
|
||||
defer e.registerMutex.Unlock()
|
||||
|
||||
if anim == nil {
|
||||
log.Printf("⚠️ 尝试注册一个空动画")
|
||||
return
|
||||
}
|
||||
|
||||
name := anim.Name()
|
||||
if _, exists := e.animations[name]; exists {
|
||||
log.Printf("⚠️ 动画 %s 已注册，将被覆盖", name)
|
||||
}
|
||||
e.animations[name] = anim
|
||||
log.Printf("✅ 动画 %s 已注册", name)
|
||||
}
|
||||
|
||||
// getAnimation 安全地获取一个已注册的动画
|
||||
func (e *AnimationEngine) getAnimation(name string) (Animation, bool) {
|
||||
e.registerMutex.RLock()
|
||||
defer e.registerMutex.RUnlock()
|
||||
anim, exists := e.animations[name]
|
||||
return anim, exists
|
||||
}
|
||||
|
||||
// getDeviceName 尝试获取设备 ID 用于日志记录
|
||||
func (e *AnimationEngine) getDeviceName() string {
|
||||
// 尝试通过接口断言获取 ID
|
||||
if idProvider, ok := e.executor.(interface{ GetID() string }); ok {
|
||||
return idProvider.GetID()
|
||||
}
|
||||
return "设备" // 默认名称
|
||||
}
|
||||
|
||||
// Start 启动一个动画
|
||||
func (e *AnimationEngine) Start(name string, speedMs int) error {
|
||||
e.engineMutex.Lock()
|
||||
defer e.engineMutex.Unlock() // 确保在任何情况下都释放锁
|
||||
|
||||
anim, exists := e.getAnimation(name)
|
||||
if !exists {
|
||||
return fmt.Errorf("❌ 动画 %s 未注册", name)
|
||||
}
|
||||
|
||||
// 如果有动画在运行，先发送停止信号
|
||||
if e.isRunning {
|
||||
log.Printf("ℹ️ 正在停止当前动画 %s 以启动 %s...", e.current, name)
|
||||
close(e.stopChan)
|
||||
// 注意：我们不在此处等待旧动画结束。
|
||||
// 新动画将立即启动，旧动画的 goroutine 在收到信号后会退出。
|
||||
// 其 defer 中的 `stopChan` 比较会确保它不会干扰新动画的状态。
|
||||
}
|
||||
|
||||
// 设置新动画状态
|
||||
e.stopChan = make(chan struct{}) // 创建新的停止通道
|
||||
e.isRunning = true
|
||||
e.current = name
|
||||
|
||||
// 验证并设置速度
|
||||
actualSpeedMs := speedMs
|
||||
if actualSpeedMs <= 0 {
|
||||
actualSpeedMs = defaultAnimationSpeedMs
|
||||
}
|
||||
|
||||
log.Printf("🚀 准备启动动画 %s (设备: %s, 速度: %dms)", name, e.getDeviceName(), actualSpeedMs)
|
||||
|
||||
// 启动动画 goroutine
|
||||
go e.runAnimationLoop(anim, e.stopChan, actualSpeedMs)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// Stop 停止当前正在运行的动画
|
||||
func (e *AnimationEngine) Stop() error {
|
||||
e.engineMutex.Lock()
|
||||
defer e.engineMutex.Unlock()
|
||||
|
||||
if !e.isRunning {
|
||||
log.Printf("ℹ️ 当前没有动画在运行 (设备: %s)", e.getDeviceName())
|
||||
return nil
|
||||
}
|
||||
|
||||
log.Printf("⏳ 正在发送停止信号给动画 %s (设备: %s)...", e.current, e.getDeviceName())
|
||||
close(e.stopChan) // 发送停止信号
|
||||
e.isRunning = false // 立即标记为未运行，防止重复停止
|
||||
e.current = ""
|
||||
// 动画的 goroutine 将在下一次检查通道时退出，
|
||||
// 并在其 defer 块中执行最终的清理（包括 ResetPose）。
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// IsRunning 检查是否有动画在运行
|
||||
func (e *AnimationEngine) IsRunning() bool {
|
||||
e.engineMutex.Lock()
|
||||
defer e.engineMutex.Unlock()
|
||||
return e.isRunning
|
||||
}
|
||||
|
||||
// runAnimationLoop 是动画执行的核心循环，在单独的 Goroutine 中运行。
|
||||
func (e *AnimationEngine) runAnimationLoop(anim Animation, stopChan <-chan struct{}, speedMs int) {
|
||||
deviceName := e.getDeviceName()
|
||||
animName := anim.Name()
|
||||
|
||||
// 使用 defer 确保无论如何都能执行清理逻辑
|
||||
defer e.handleLoopExit(stopChan, deviceName, animName)
|
||||
|
||||
log.Printf("▶️ %s 动画 %s 已启动", deviceName, animName)
|
||||
|
||||
// 动画主循环
|
||||
for {
|
||||
select {
|
||||
case <-stopChan:
|
||||
log.Printf("🛑 %s 动画 %s 被显式停止", deviceName, animName)
|
||||
return // 接收到停止信号，退出循环
|
||||
default:
|
||||
// 执行一轮动画
|
||||
err := anim.Run(e.executor, stopChan, speedMs)
|
||||
if err != nil {
|
||||
log.Printf("❌ %s 动画 %s 执行出错: %v", deviceName, animName, err)
|
||||
return // 出错则退出
|
||||
}
|
||||
|
||||
// 再次检查停止信号，防止 Run 结束后才收到信号
|
||||
select {
|
||||
case <-stopChan:
|
||||
log.Printf("🛑 %s 动画 %s 在周期结束时被停止", deviceName, animName)
|
||||
return
|
||||
default:
|
||||
// 继续下一个循环
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// handleLoopExit 是动画 Goroutine 退出时执行的清理函数。
|
||||
func (e *AnimationEngine) handleLoopExit(stopChan <-chan struct{}, deviceName, animName string) {
|
||||
e.engineMutex.Lock()
|
||||
defer e.engineMutex.Unlock()
|
||||
|
||||
// --- 关键并发控制 ---
|
||||
// 检查当前引擎的 stopChan 是否与此 Goroutine 启动时的 stopChan 相同。
|
||||
// 如果不相同，说明一个新的动画已经启动，并且接管了引擎状态。
|
||||
// 这种情况下，旧的 Goroutine 不应该修改引擎状态或重置姿态，
|
||||
// 以避免干扰新动画。
|
||||
if stopChan == e.stopChan {
|
||||
// 只有当自己仍然是“活跃”的动画时，才更新状态并重置姿态
|
||||
e.isRunning = false
|
||||
e.current = ""
|
||||
log.Printf("👋 %s 动画 %s 已完成或停止，正在重置姿态...", deviceName, animName)
|
||||
if err := e.executor.ResetPose(); err != nil {
|
||||
log.Printf("⚠️ %s 动画结束后重置姿态失败: %v", deviceName, err)
|
||||
} else {
|
||||
log.Printf("✅ %s 姿态已重置", deviceName)
|
||||
}
|
||||
} else {
|
||||
// 如果 stopChan 不同，说明自己是旧的 Goroutine，只需安静退出
|
||||
log.Printf("ℹ️ 旧的 %s 动画 %s goroutine 退出，但新动画已启动，无需重置。", deviceName, animName)
|
||||
}
|
||||
}
|
||||
@ -2,6 +2,8 @@ package models
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"math/rand/v2"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
@ -13,14 +15,28 @@ import (
|
||||
|
||||
// L10Hand L10 型号手部设备实现
|
||||
type L10Hand struct {
|
||||
id string
|
||||
model string
|
||||
handType define.HandType
|
||||
communicator communication.Communicator
|
||||
components map[device.ComponentType][]device.Component
|
||||
status device.DeviceStatus
|
||||
mutex sync.RWMutex
|
||||
canInterface string // CAN 接口名称，如 "can0"
|
||||
id string
|
||||
model string
|
||||
handType define.HandType
|
||||
communicator communication.Communicator
|
||||
components map[device.ComponentType][]device.Component
|
||||
status device.DeviceStatus
|
||||
mutex sync.RWMutex
|
||||
canInterface string // CAN 接口名称，如 "can0"
|
||||
animationEngine *device.AnimationEngine // 动画引擎
|
||||
}
|
||||
|
||||
// 在 base 基础上进行 ±delta 的扰动，范围限制在 [0, 255]
|
||||
func perturb(base byte, delta int) byte {
|
||||
offset := rand.IntN(2*delta+1) - delta
|
||||
v := int(base) + offset
|
||||
if v < 0 {
|
||||
v = 0
|
||||
}
|
||||
if v > 255 {
|
||||
v = 255
|
||||
}
|
||||
return byte(v)
|
||||
}
|
||||
|
||||
// NewL10Hand 创建 L10 手部设备实例
|
||||
@ -63,23 +79,179 @@ func NewL10Hand(config map[string]any) (device.Device, error) {
|
||||
},
|
||||
}
|
||||
|
||||
// 初始化动画引擎，将 hand 自身作为 PoseExecutor
|
||||
hand.animationEngine = device.NewAnimationEngine(hand)
|
||||
|
||||
// 注册默认动画
|
||||
hand.animationEngine.Register(NewL10WaveAnimation())
|
||||
hand.animationEngine.Register(NewL10SwayAnimation())
|
||||
|
||||
// 初始化组件
|
||||
if err := hand.initializeComponents(config); err != nil {
|
||||
return nil, fmt.Errorf("初始化组件失败：%w", err)
|
||||
}
|
||||
|
||||
log.Printf("✅ 设备 L10 (%s, %s) 创建成功", id, handType.String())
|
||||
return hand, nil
|
||||
}
|
||||
|
||||
// GetHandType 获取设备手型
|
||||
func (h *L10Hand) GetHandType() define.HandType {
|
||||
h.mutex.RLock()
|
||||
defer h.mutex.RUnlock()
|
||||
return h.handType
|
||||
}
|
||||
|
||||
// SetHandType 设置设备手型
|
||||
func (h *L10Hand) SetHandType(handType define.HandType) error {
|
||||
h.mutex.Lock()
|
||||
defer h.mutex.Unlock()
|
||||
if handType != define.HAND_TYPE_LEFT && handType != define.HAND_TYPE_RIGHT {
|
||||
return fmt.Errorf("无效的手型：%d", handType)
|
||||
}
|
||||
h.handType = handType
|
||||
log.Printf("🔧 设备 %s 手型已更新: %s", h.id, handType.String())
|
||||
return nil
|
||||
}
|
||||
|
||||
// GetAnimationEngine 获取动画引擎
|
||||
func (h *L10Hand) GetAnimationEngine() *device.AnimationEngine {
|
||||
return h.animationEngine
|
||||
}
|
||||
|
||||
// SetFingerPose 设置手指姿态 (实现 PoseExecutor)
|
||||
func (h *L10Hand) SetFingerPose(pose []byte) error {
|
||||
if len(pose) != 6 {
|
||||
return fmt.Errorf("无效的手指姿态数据长度，需要 6 个字节")
|
||||
}
|
||||
|
||||
// 添加随机扰动
|
||||
perturbedPose := make([]byte, len(pose))
|
||||
for i, v := range pose {
|
||||
perturbedPose[i] = perturb(v, 5)
|
||||
}
|
||||
|
||||
// 创建指令
|
||||
cmd := device.NewFingerPoseCommand("all", perturbedPose)
|
||||
|
||||
// 执行指令
|
||||
err := h.ExecuteCommand(cmd)
|
||||
if err == nil {
|
||||
log.Printf("✅ %s (%s) 手指动作已发送: [%X %X %X %X %X %X]",
|
||||
h.id, h.GetHandType().String(), perturbedPose[0], perturbedPose[1], perturbedPose[2],
|
||||
perturbedPose[3], perturbedPose[4], perturbedPose[5])
|
||||
}
|
||||
return err
|
||||
}
|
||||
|
||||
// SetPalmPose 设置手掌姿态 (实现 PoseExecutor)
|
||||
func (h *L10Hand) SetPalmPose(pose []byte) error {
|
||||
if len(pose) != 4 {
|
||||
return fmt.Errorf("无效的手掌姿态数据长度，需要 4 个字节")
|
||||
}
|
||||
|
||||
// 添加随机扰动
|
||||
perturbedPose := make([]byte, len(pose))
|
||||
for i, v := range pose {
|
||||
perturbedPose[i] = perturb(v, 8)
|
||||
}
|
||||
|
||||
// 创建指令
|
||||
cmd := device.NewPalmPoseCommand(perturbedPose)
|
||||
|
||||
// 执行指令
|
||||
err := h.ExecuteCommand(cmd)
|
||||
if err == nil {
|
||||
log.Printf("✅ %s (%s) 掌部姿态已发送: [%X %X %X %X]",
|
||||
h.id, h.GetHandType().String(), perturbedPose[0], perturbedPose[1], perturbedPose[2], perturbedPose[3])
|
||||
}
|
||||
return err
|
||||
}
|
||||
|
||||
// ResetPose 重置到默认姿态 (实现 PoseExecutor)
|
||||
func (h *L10Hand) ResetPose() error {
|
||||
log.Printf("🔄 正在重置设备 %s (%s) 到默认姿态...", h.id, h.GetHandType().String())
|
||||
defaultFingerPose := []byte{64, 64, 64, 64, 64, 64} // 0x40 - 半开
|
||||
defaultPalmPose := []byte{128, 128, 128, 128} // 0x80 - 居中
|
||||
|
||||
if err := h.SetFingerPose(defaultFingerPose); err != nil {
|
||||
log.Printf("❌ %s 重置手指姿势失败: %v", h.id, err)
|
||||
return err
|
||||
}
|
||||
time.Sleep(20 * time.Millisecond) // 短暂延时
|
||||
if err := h.SetPalmPose(defaultPalmPose); err != nil {
|
||||
log.Printf("❌ %s 重置掌部姿势失败: %v", h.id, err)
|
||||
return err
|
||||
}
|
||||
log.Printf("✅ 设备 %s 已重置到默认姿态", h.id)
|
||||
return nil
|
||||
}
|
||||
|
||||
// commandToRawMessage 将通用指令转换为 L10 特定的 CAN 消息
|
||||
func (h *L10Hand) commandToRawMessage(cmd device.Command) (communication.RawMessage, error) {
|
||||
h.mutex.RLock()
|
||||
defer h.mutex.RUnlock()
|
||||
|
||||
var data []byte
|
||||
canID := uint32(h.handType)
|
||||
|
||||
switch cmd.Type() {
|
||||
case "SetFingerPose":
|
||||
// 添加 0x01 前缀
|
||||
data = append([]byte{0x01}, cmd.Payload()...)
|
||||
if len(data) > 8 { // CAN 消息数据长度限制
|
||||
return communication.RawMessage{}, fmt.Errorf("手指姿态数据过长")
|
||||
}
|
||||
case "SetPalmPose":
|
||||
// 添加 0x04 前缀
|
||||
data = append([]byte{0x04}, cmd.Payload()...)
|
||||
if len(data) > 8 { // CAN 消息数据长度限制
|
||||
return communication.RawMessage{}, fmt.Errorf("手掌姿态数据过长")
|
||||
}
|
||||
default:
|
||||
return communication.RawMessage{}, fmt.Errorf("L10 不支持的指令类型: %s", cmd.Type())
|
||||
}
|
||||
|
||||
return communication.RawMessage{
|
||||
Interface: h.canInterface,
|
||||
ID: canID,
|
||||
Data: data,
|
||||
}, nil
|
||||
}
|
||||
|
||||
// ExecuteCommand 执行一个通用指令
|
||||
func (h *L10Hand) ExecuteCommand(cmd device.Command) error {
|
||||
h.mutex.Lock() // 使用写锁，因为会更新状态
|
||||
defer h.mutex.Unlock()
|
||||
|
||||
if !h.status.IsConnected || !h.status.IsActive {
|
||||
return fmt.Errorf("设备 %s 未连接或未激活", h.id)
|
||||
}
|
||||
|
||||
// 转换指令为 CAN 消息
|
||||
rawMsg, err := h.commandToRawMessage(cmd)
|
||||
if err != nil {
|
||||
h.status.ErrorCount++
|
||||
h.status.LastError = err.Error()
|
||||
return fmt.Errorf("转换指令失败：%w", err)
|
||||
}
|
||||
|
||||
// 发送到 can-bridge 服务
|
||||
if err := h.communicator.SendMessage(rawMsg); err != nil {
|
||||
h.status.ErrorCount++
|
||||
h.status.LastError = err.Error()
|
||||
log.Printf("❌ %s (%s) 发送指令失败: %v (ID: 0x%X, Data: %X)", h.id, h.handType.String(), err, rawMsg.ID, rawMsg.Data)
|
||||
return fmt.Errorf("发送指令失败：%w", err)
|
||||
}
|
||||
|
||||
h.status.LastUpdate = time.Now()
|
||||
// 成功的日志记录移到 SetFingerPose 和 SetPalmPose 中，因为那里有更详细的信息
|
||||
return nil
|
||||
}
|
||||
|
||||
// --- 其他 L10Hand 方法 (initializeComponents, GetID, GetModel, ReadSensorData, etc.) 保持不变 ---
|
||||
// --- 确保它们存在且与您上传的版本一致 ---
|
||||
|
||||
func (h *L10Hand) initializeComponents(_ map[string]any) error {
|
||||
// 初始化传感器组件
|
||||
sensors := []device.Component{
|
||||
@ -90,7 +262,6 @@ func (h *L10Hand) initializeComponents(_ map[string]any) error {
|
||||
component.NewPressureSensor("pressure_pinky", map[string]any{"location": "pinky"}),
|
||||
}
|
||||
h.components[device.SensorComponent] = sensors
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
@ -102,75 +273,10 @@ func (h *L10Hand) GetModel() string {
|
||||
return h.model
|
||||
}
|
||||
|
||||
func (h *L10Hand) ExecuteCommand(cmd device.Command) error {
|
||||
h.mutex.Lock()
|
||||
defer h.mutex.Unlock()
|
||||
|
||||
// 将通用指令转换为 L10 特定的 CAN 消息
|
||||
rawMsg, err := h.commandToRawMessage(cmd)
|
||||
if err != nil {
|
||||
return fmt.Errorf("转换指令失败：%w", err)
|
||||
}
|
||||
|
||||
// 发送到 can-bridge 服务
|
||||
if err := h.communicator.SendMessage(rawMsg); err != nil {
|
||||
h.status.ErrorCount++
|
||||
h.status.LastError = err.Error()
|
||||
return fmt.Errorf("发送指令失败：%w", err)
|
||||
}
|
||||
|
||||
h.status.LastUpdate = time.Now()
|
||||
return nil
|
||||
}
|
||||
|
||||
func (h *L10Hand) commandToRawMessage(cmd device.Command) (communication.RawMessage, error) {
|
||||
var canID uint32
|
||||
var data []byte
|
||||
|
||||
switch cmd.Type() {
|
||||
case "SetFingerPose":
|
||||
// 根据目标组件确定 CAN ID
|
||||
canID = h.getFingerCanID(cmd.TargetComponent())
|
||||
data = cmd.Payload()
|
||||
case "SetPalmPose":
|
||||
canID = h.getPalmCanID()
|
||||
data = cmd.Payload()
|
||||
default:
|
||||
return communication.RawMessage{}, fmt.Errorf("不支持的指令类型: %s", cmd.Type())
|
||||
}
|
||||
|
||||
return communication.RawMessage{
|
||||
Interface: h.canInterface,
|
||||
ID: canID,
|
||||
Data: data,
|
||||
}, nil
|
||||
}
|
||||
|
||||
func (h *L10Hand) getFingerCanID(targetComponent string) uint32 {
|
||||
// L10 设备的手指 CAN ID 映射
|
||||
fingerIDs := map[string]uint32{
|
||||
"finger_thumb": 0x100,
|
||||
"finger_index": 0x101,
|
||||
"finger_middle": 0x102,
|
||||
"finger_ring": 0x103,
|
||||
"finger_pinky": 0x104,
|
||||
}
|
||||
|
||||
if id, exists := fingerIDs[targetComponent]; exists {
|
||||
return id
|
||||
}
|
||||
return 0x100 // 默认拇指
|
||||
}
|
||||
|
||||
func (h *L10Hand) getPalmCanID() uint32 {
|
||||
return 0x200 // L10 设备的手掌 CAN ID
|
||||
}
|
||||
|
||||
func (h *L10Hand) ReadSensorData(sensorID string) (device.SensorData, error) {
|
||||
h.mutex.RLock()
|
||||
defer h.mutex.RUnlock()
|
||||
|
||||
// 查找传感器组件
|
||||
sensors := h.components[device.SensorComponent]
|
||||
for _, comp := range sensors {
|
||||
if comp.GetID() == sensorID {
|
||||
@ -179,7 +285,6 @@ func (h *L10Hand) ReadSensorData(sensorID string) (device.SensorData, error) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return nil, fmt.Errorf("传感器 %s 不存在", sensorID)
|
||||
}
|
||||
|
||||
@ -192,14 +297,12 @@ func (h *L10Hand) GetComponents(componentType device.ComponentType) []device.Com
|
||||
copy(result, components)
|
||||
return result
|
||||
}
|
||||
|
||||
return []device.Component{}
|
||||
}
|
||||
|
||||
func (h *L10Hand) GetStatus() (device.DeviceStatus, error) {
|
||||
h.mutex.RLock()
|
||||
defer h.mutex.RUnlock()
|
||||
|
||||
return h.status, nil
|
||||
}
|
||||
|
||||
@ -207,25 +310,11 @@ func (h *L10Hand) Connect() error {
|
||||
h.mutex.Lock()
|
||||
defer h.mutex.Unlock()
|
||||
|
||||
// 检查与 can-bridge 服务的连接
|
||||
if !h.communicator.IsConnected() {
|
||||
return fmt.Errorf("无法连接到 can-bridge 服务")
|
||||
}
|
||||
|
||||
// 检查 CAN 接口状态
|
||||
isActive, err := h.communicator.GetInterfaceStatus(h.canInterface)
|
||||
if err != nil {
|
||||
return fmt.Errorf("检查 CAN 接口状态失败：%w", err)
|
||||
}
|
||||
|
||||
if !isActive {
|
||||
return fmt.Errorf("CAN接口 %s 未激活", h.canInterface)
|
||||
}
|
||||
|
||||
// TODO: 假设连接总是成功，除非有显式错误
|
||||
h.status.IsConnected = true
|
||||
h.status.IsActive = true
|
||||
h.status.LastUpdate = time.Now()
|
||||
|
||||
log.Printf("🔗 设备 %s 已连接", h.id)
|
||||
return nil
|
||||
}
|
||||
|
||||
@ -236,6 +325,6 @@ func (h *L10Hand) Disconnect() error {
|
||||
h.status.IsConnected = false
|
||||
h.status.IsActive = false
|
||||
h.status.LastUpdate = time.Now()
|
||||
|
||||
log.Printf("🔌 设备 %s 已断开", h.id)
|
||||
return nil
|
||||
}
|
||||
|
||||
125
pkg/device/models/l10_animation.go
Normal file
125
pkg/device/models/l10_animation.go
Normal file
@ -0,0 +1,125 @@
|
||||
package models
|
||||
|
||||
import (
|
||||
"hands/pkg/device"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
// --- L10WaveAnimation ---
|
||||
|
||||
// L10WaveAnimation 实现 L10 的波浪动画
|
||||
type L10WaveAnimation struct{}
|
||||
|
||||
// NewL10WaveAnimation 创建 L10 波浪动画实例
|
||||
func NewL10WaveAnimation() *L10WaveAnimation { return &L10WaveAnimation{} }
|
||||
|
||||
func (w *L10WaveAnimation) Name() string { return "wave" }
|
||||
|
||||
func (w *L10WaveAnimation) Run(executor device.PoseExecutor, stop <-chan struct{}, speedMs int) error {
|
||||
fingerOrder := []int{0, 1, 2, 3, 4, 5}
|
||||
open := byte(64) // 0x40
|
||||
close := byte(192) // 0xC0
|
||||
delay := time.Duration(speedMs) * time.Millisecond
|
||||
|
||||
deviceName := "L10"
|
||||
|
||||
// 波浪张开
|
||||
for _, idx := range fingerOrder {
|
||||
pose := make([]byte, 6)
|
||||
for j := 0; j < 6; j++ {
|
||||
if j == idx {
|
||||
pose[j] = open
|
||||
} else {
|
||||
pose[j] = close
|
||||
}
|
||||
}
|
||||
|
||||
if err := executor.SetFingerPose(pose); err != nil {
|
||||
log.Printf("❌ %s 动画 %s 发送失败: %v", deviceName, w.Name(), err)
|
||||
return err
|
||||
}
|
||||
|
||||
select {
|
||||
case <-stop:
|
||||
return nil // 动画被停止
|
||||
case <-time.After(delay):
|
||||
// 继续
|
||||
}
|
||||
}
|
||||
|
||||
// 波浪握拳
|
||||
for _, idx := range fingerOrder {
|
||||
pose := make([]byte, 6)
|
||||
for j := 0; j < 6; j++ {
|
||||
if j == idx {
|
||||
pose[j] = close
|
||||
} else {
|
||||
pose[j] = open
|
||||
}
|
||||
}
|
||||
|
||||
if err := executor.SetFingerPose(pose); err != nil {
|
||||
log.Printf("❌ %s 动画 %s 发送失败: %v", deviceName, w.Name(), err)
|
||||
return err
|
||||
}
|
||||
|
||||
select {
|
||||
case <-stop:
|
||||
return nil // 动画被停止
|
||||
case <-time.After(delay):
|
||||
// 继续
|
||||
}
|
||||
}
|
||||
|
||||
return nil // 完成一个周期
|
||||
}
|
||||
|
||||
// --- L10SwayAnimation ---
|
||||
|
||||
// L10SwayAnimation 实现 L10 的横向摆动动画
|
||||
type L10SwayAnimation struct{}
|
||||
|
||||
// NewL10SwayAnimation 创建 L10 摆动动画实例
|
||||
func NewL10SwayAnimation() *L10SwayAnimation { return &L10SwayAnimation{} }
|
||||
|
||||
func (s *L10SwayAnimation) Name() string { return "sway" }
|
||||
|
||||
func (s *L10SwayAnimation) Run(executor device.PoseExecutor, stop <-chan struct{}, speedMs int) error {
|
||||
leftPose := []byte{48, 48, 48, 48} // 0x30
|
||||
rightPose := []byte{208, 208, 208, 208} // 0xD0
|
||||
delay := time.Duration(speedMs) * time.Millisecond
|
||||
|
||||
deviceName := "L10"
|
||||
if idProvider, ok := executor.(interface{ GetID() string }); ok {
|
||||
deviceName = idProvider.GetID()
|
||||
}
|
||||
|
||||
// 向左移动
|
||||
if err := executor.SetPalmPose(leftPose); err != nil {
|
||||
log.Printf("❌ %s 动画 %s 发送失败: %v", deviceName, s.Name(), err)
|
||||
return err
|
||||
}
|
||||
|
||||
select {
|
||||
case <-stop:
|
||||
return nil // 动画被停止
|
||||
case <-time.After(delay):
|
||||
// 继续
|
||||
}
|
||||
|
||||
// 向右移动
|
||||
if err := executor.SetPalmPose(rightPose); err != nil {
|
||||
log.Printf("❌ %s 动画 %s 发送失败: %v", deviceName, s.Name(), err)
|
||||
return err
|
||||
}
|
||||
|
||||
select {
|
||||
case <-stop:
|
||||
return nil // 动画被停止
|
||||
case <-time.After(delay):
|
||||
// 继续
|
||||
}
|
||||
|
||||
return nil // 完成一个周期
|
||||
}
|
||||
20
pkg/device/pose_executor.go
Normal file
20
pkg/device/pose_executor.go
Normal file
@ -0,0 +1,20 @@
|
||||
package device
|
||||
|
||||
import "hands/define"
|
||||
|
||||
// PoseExecutor 定义了执行基本姿态指令的能力
|
||||
type PoseExecutor interface {
|
||||
// SetFingerPose 设置手指姿态
|
||||
// pose: 6 字节数据，代表 6 个手指的位置
|
||||
SetFingerPose(pose []byte) error
|
||||
|
||||
// SetPalmPose 设置手掌姿态
|
||||
// pose: 4 字节数据，代表手掌的 4 个自由度
|
||||
SetPalmPose(pose []byte) error
|
||||
|
||||
// ResetPose 重置到默认姿态
|
||||
ResetPose() error
|
||||
|
||||
// GetHandType 获取当前手型
|
||||
GetHandType() define.HandType
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user