feat: implement new animation iface
This commit is contained in:
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) // 获取设备状态
|
GetStatus() (DeviceStatus, error) // 获取设备状态
|
||||||
Connect() error // 连接设备
|
Connect() error // 连接设备
|
||||||
Disconnect() error // 断开设备连接
|
Disconnect() error // 断开设备连接
|
||||||
|
|
||||||
|
// --- 新增 ---
|
||||||
|
PoseExecutor // 嵌入 PoseExecutor 接口,Device 需实现它
|
||||||
|
GetAnimationEngine() *AnimationEngine // 获取设备的动画引擎
|
||||||
}
|
}
|
||||||
|
|
||||||
// Command 代表一个发送给设备的指令
|
// 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 (
|
import (
|
||||||
"fmt"
|
"fmt"
|
||||||
|
"log"
|
||||||
|
"math/rand/v2"
|
||||||
"sync"
|
"sync"
|
||||||
"time"
|
"time"
|
||||||
|
|
||||||
@ -13,14 +15,28 @@ import (
|
|||||||
|
|
||||||
// L10Hand L10 型号手部设备实现
|
// L10Hand L10 型号手部设备实现
|
||||||
type L10Hand struct {
|
type L10Hand struct {
|
||||||
id string
|
id string
|
||||||
model string
|
model string
|
||||||
handType define.HandType
|
handType define.HandType
|
||||||
communicator communication.Communicator
|
communicator communication.Communicator
|
||||||
components map[device.ComponentType][]device.Component
|
components map[device.ComponentType][]device.Component
|
||||||
status device.DeviceStatus
|
status device.DeviceStatus
|
||||||
mutex sync.RWMutex
|
mutex sync.RWMutex
|
||||||
canInterface string // CAN 接口名称,如 "can0"
|
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 手部设备实例
|
// 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 {
|
if err := hand.initializeComponents(config); err != nil {
|
||||||
return nil, fmt.Errorf("初始化组件失败:%w", err)
|
return nil, fmt.Errorf("初始化组件失败:%w", err)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
log.Printf("✅ 设备 L10 (%s, %s) 创建成功", id, handType.String())
|
||||||
return hand, nil
|
return hand, nil
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// GetHandType 获取设备手型
|
||||||
func (h *L10Hand) GetHandType() define.HandType {
|
func (h *L10Hand) GetHandType() define.HandType {
|
||||||
|
h.mutex.RLock()
|
||||||
|
defer h.mutex.RUnlock()
|
||||||
return h.handType
|
return h.handType
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// SetHandType 设置设备手型
|
||||||
func (h *L10Hand) SetHandType(handType define.HandType) error {
|
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
|
h.handType = handType
|
||||||
|
log.Printf("🔧 设备 %s 手型已更新: %s", h.id, handType.String())
|
||||||
return nil
|
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 {
|
func (h *L10Hand) initializeComponents(_ map[string]any) error {
|
||||||
// 初始化传感器组件
|
// 初始化传感器组件
|
||||||
sensors := []device.Component{
|
sensors := []device.Component{
|
||||||
@ -90,7 +262,6 @@ func (h *L10Hand) initializeComponents(_ map[string]any) error {
|
|||||||
component.NewPressureSensor("pressure_pinky", map[string]any{"location": "pinky"}),
|
component.NewPressureSensor("pressure_pinky", map[string]any{"location": "pinky"}),
|
||||||
}
|
}
|
||||||
h.components[device.SensorComponent] = sensors
|
h.components[device.SensorComponent] = sensors
|
||||||
|
|
||||||
return nil
|
return nil
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -102,75 +273,10 @@ func (h *L10Hand) GetModel() string {
|
|||||||
return h.model
|
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) {
|
func (h *L10Hand) ReadSensorData(sensorID string) (device.SensorData, error) {
|
||||||
h.mutex.RLock()
|
h.mutex.RLock()
|
||||||
defer h.mutex.RUnlock()
|
defer h.mutex.RUnlock()
|
||||||
|
|
||||||
// 查找传感器组件
|
|
||||||
sensors := h.components[device.SensorComponent]
|
sensors := h.components[device.SensorComponent]
|
||||||
for _, comp := range sensors {
|
for _, comp := range sensors {
|
||||||
if comp.GetID() == sensorID {
|
if comp.GetID() == sensorID {
|
||||||
@ -179,7 +285,6 @@ func (h *L10Hand) ReadSensorData(sensorID string) (device.SensorData, error) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return nil, fmt.Errorf("传感器 %s 不存在", sensorID)
|
return nil, fmt.Errorf("传感器 %s 不存在", sensorID)
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -192,14 +297,12 @@ func (h *L10Hand) GetComponents(componentType device.ComponentType) []device.Com
|
|||||||
copy(result, components)
|
copy(result, components)
|
||||||
return result
|
return result
|
||||||
}
|
}
|
||||||
|
|
||||||
return []device.Component{}
|
return []device.Component{}
|
||||||
}
|
}
|
||||||
|
|
||||||
func (h *L10Hand) GetStatus() (device.DeviceStatus, error) {
|
func (h *L10Hand) GetStatus() (device.DeviceStatus, error) {
|
||||||
h.mutex.RLock()
|
h.mutex.RLock()
|
||||||
defer h.mutex.RUnlock()
|
defer h.mutex.RUnlock()
|
||||||
|
|
||||||
return h.status, nil
|
return h.status, nil
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -207,25 +310,11 @@ func (h *L10Hand) Connect() error {
|
|||||||
h.mutex.Lock()
|
h.mutex.Lock()
|
||||||
defer h.mutex.Unlock()
|
defer h.mutex.Unlock()
|
||||||
|
|
||||||
// 检查与 can-bridge 服务的连接
|
// TODO: 假设连接总是成功,除非有显式错误
|
||||||
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)
|
|
||||||
}
|
|
||||||
|
|
||||||
h.status.IsConnected = true
|
h.status.IsConnected = true
|
||||||
h.status.IsActive = true
|
h.status.IsActive = true
|
||||||
h.status.LastUpdate = time.Now()
|
h.status.LastUpdate = time.Now()
|
||||||
|
log.Printf("🔗 设备 %s 已连接", h.id)
|
||||||
return nil
|
return nil
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -236,6 +325,6 @@ func (h *L10Hand) Disconnect() error {
|
|||||||
h.status.IsConnected = false
|
h.status.IsConnected = false
|
||||||
h.status.IsActive = false
|
h.status.IsActive = false
|
||||||
h.status.LastUpdate = time.Now()
|
h.status.LastUpdate = time.Now()
|
||||||
|
log.Printf("🔌 设备 %s 已断开", h.id)
|
||||||
return nil
|
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