From 553873799ea944512ad9960c2c4aa1979382e672 Mon Sep 17 00:00:00 2001 From: Eli Yip Date: Tue, 27 May 2025 15:24:23 +0800 Subject: [PATCH] feat: implement device and l10 model --- pkg/device/commands.go | 80 +++++++++++++ pkg/device/device.go | 60 ++++++++++ pkg/device/factory.go | 35 ++++++ pkg/device/models/init.go | 8 ++ pkg/device/models/l10.go | 240 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 423 insertions(+) create mode 100644 pkg/device/commands.go create mode 100644 pkg/device/device.go create mode 100644 pkg/device/factory.go create mode 100644 pkg/device/models/init.go create mode 100644 pkg/device/models/l10.go diff --git a/pkg/device/commands.go b/pkg/device/commands.go new file mode 100644 index 0000000..fb14a71 --- /dev/null +++ b/pkg/device/commands.go @@ -0,0 +1,80 @@ +package device + +// FingerPoseCommand 手指姿态指令 +type FingerPoseCommand struct { + fingerID string + poseData []byte + targetComp string +} + +func NewFingerPoseCommand(fingerID string, poseData []byte) *FingerPoseCommand { + return &FingerPoseCommand{ + fingerID: fingerID, + poseData: poseData, + targetComp: "finger_" + fingerID, + } +} + +func (c *FingerPoseCommand) Type() string { + return "SetFingerPose" +} + +func (c *FingerPoseCommand) Payload() []byte { + return c.poseData +} + +func (c *FingerPoseCommand) TargetComponent() string { + return c.targetComp +} + +// PalmPoseCommand 手掌姿态指令 +type PalmPoseCommand struct { + poseData []byte + targetComp string +} + +func NewPalmPoseCommand(poseData []byte) *PalmPoseCommand { + return &PalmPoseCommand{ + poseData: poseData, + targetComp: "palm", + } +} + +func (c *PalmPoseCommand) Type() string { + return "SetPalmPose" +} + +func (c *PalmPoseCommand) Payload() []byte { + return c.poseData +} + +func (c *PalmPoseCommand) TargetComponent() string { + return c.targetComp +} + +// GenericCommand 通用指令 +type GenericCommand struct { + cmdType string + payload []byte + targetComp string +} + +func NewGenericCommand(cmdType string, payload []byte, targetComp string) *GenericCommand { + return &GenericCommand{ + cmdType: cmdType, + payload: payload, + targetComp: targetComp, + } +} + +func (c *GenericCommand) Type() string { + return c.cmdType +} + +func (c *GenericCommand) Payload() []byte { + return c.payload +} + +func (c *GenericCommand) TargetComponent() string { + return c.targetComp +} diff --git a/pkg/device/device.go b/pkg/device/device.go new file mode 100644 index 0000000..0a4d31c --- /dev/null +++ b/pkg/device/device.go @@ -0,0 +1,60 @@ +package device + +import ( + "hands/define" + "time" +) + +// Device 代表一个可控制的设备单元 +type Device interface { + GetID() string // 获取设备唯一标识 + GetModel() string // 获取设备型号 (例如 "L10", "L20") + GetHandType() define.HandType // 获取设备手型 + SetHandType(handType define.HandType) error // 设置设备手型 + ExecuteCommand(cmd Command) error // 执行一个通用指令 + ReadSensorData(sensorID string) (SensorData, error) // 读取特定传感器数据 + GetComponents(componentType ComponentType) []Component // 获取指定类型的组件 + GetStatus() (DeviceStatus, error) // 获取设备状态 + Connect() error // 连接设备 + Disconnect() error // 断开设备连接 +} + +// Command 代表一个发送给设备的指令 +type Command interface { + Type() string // 指令类型，例如 "SetFingerPose", "SetPalmAngle" + Payload() []byte // 指令的实际数据 + TargetComponent() string // 目标组件 ID +} + +// SensorData 代表从传感器读取的数据 +type SensorData interface { + Timestamp() time.Time + Values() map[string]any // 例如 {"pressure": 100, "angle": 30.5} + SensorID() string +} + +// ComponentType 定义组件类型 +type ComponentType string + +const ( + SensorComponent ComponentType = "sensor" + SkinComponent ComponentType = "skin" + ActuatorComponent ComponentType = "actuator" +) + +// Component 代表设备的一个可插拔组件 +type Component interface { + GetID() string + GetType() ComponentType + GetConfiguration() map[string]interface{} // 组件的特定配置 + IsActive() bool +} + +// DeviceStatus 代表设备状态 +type DeviceStatus struct { + IsConnected bool + IsActive bool + LastUpdate time.Time + ErrorCount int + LastError string +} diff --git a/pkg/device/factory.go b/pkg/device/factory.go new file mode 100644 index 0000000..b147563 --- /dev/null +++ b/pkg/device/factory.go @@ -0,0 +1,35 @@ +package device + +import "fmt" + +// DeviceFactory 设备工厂 +type DeviceFactory struct { + constructors map[string]func(config map[string]any) (Device, error) +} + +var defaultFactory = &DeviceFactory{ + constructors: make(map[string]func(config map[string]any) (Device, error)), +} + +// RegisterDeviceType 注册设备类型 +func RegisterDeviceType(modelName string, constructor func(config map[string]any) (Device, error)) { + defaultFactory.constructors[modelName] = constructor +} + +// CreateDevice 创建设备实例 +func CreateDevice(modelName string, config map[string]any) (Device, error) { + constructor, ok := defaultFactory.constructors[modelName] + if !ok { + return nil, fmt.Errorf("未知的设备型号: %s", modelName) + } + return constructor(config) +} + +// GetSupportedModels 获取支持的设备型号列表 +func GetSupportedModels() []string { + models := make([]string, 0, len(defaultFactory.constructors)) + for model := range defaultFactory.constructors { + models = append(models, model) + } + return models +} diff --git a/pkg/device/models/init.go b/pkg/device/models/init.go new file mode 100644 index 0000000..c3910c7 --- /dev/null +++ b/pkg/device/models/init.go @@ -0,0 +1,8 @@ +package models + +import "hands/pkg/device" + +func init() { + // 注册 L10 设备类型 + device.RegisterDeviceType("L10", NewL10Hand) +} diff --git a/pkg/device/models/l10.go b/pkg/device/models/l10.go new file mode 100644 index 0000000..1711d58 --- /dev/null +++ b/pkg/device/models/l10.go @@ -0,0 +1,240 @@ +package models + +import ( + "fmt" + "sync" + "time" + + "hands/define" + "hands/pkg/communication" + "hands/pkg/component" + "hands/pkg/device" +) + +// 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" +} + +// NewL10Hand 创建 L10 手部设备实例 +func NewL10Hand(config map[string]any) (device.Device, error) { + id, ok := config["id"].(string) + if !ok { + return nil, fmt.Errorf("缺少设备 ID 配置") + } + + serviceURL, ok := config["can_service_url"].(string) + if !ok { + return nil, fmt.Errorf("缺少 can 服务 URL 配置") + } + + canInterface, ok := config["can_interface"].(string) + if !ok { + canInterface = "can0" // 默认接口 + } + + handType, ok := config["hand_type"].(define.HandType) + if !ok { + handType = define.HAND_TYPE_LEFT + } + + // 创建通信客户端 + comm := communication.NewCanBridgeClient(serviceURL) + + hand := &L10Hand{ + id: id, + model: "L10", + handType: handType, + communicator: comm, + components: make(map[device.ComponentType][]device.Component), + canInterface: canInterface, + status: device.DeviceStatus{ + IsConnected: false, + IsActive: false, + LastUpdate: time.Now(), + }, + } + + // 初始化组件 + if err := hand.initializeComponents(config); err != nil { + return nil, fmt.Errorf("初始化组件失败：%w", err) + } + + return hand, nil +} + +func (h *L10Hand) GetHandType() define.HandType { + return h.handType +} + +func (h *L10Hand) SetHandType(handType define.HandType) error { + h.handType = handType + return nil +} + +func (h *L10Hand) initializeComponents(_ map[string]any) error { + // 初始化传感器组件 + sensors := []device.Component{ + component.NewPressureSensor("pressure_thumb", map[string]any{"location": "thumb"}), + component.NewPressureSensor("pressure_index", map[string]any{"location": "index"}), + component.NewPressureSensor("pressure_middle", map[string]any{"location": "middle"}), + component.NewPressureSensor("pressure_ring", map[string]any{"location": "ring"}), + component.NewPressureSensor("pressure_pinky", map[string]any{"location": "pinky"}), + } + h.components[device.SensorComponent] = sensors + + return nil +} + +func (h *L10Hand) GetID() string { + return h.id +} + +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 { + if sensor, ok := comp.(component.Sensor); ok { + return sensor.ReadData() + } + } + } + + return nil, fmt.Errorf("传感器 %s 不存在", sensorID) +} + +func (h *L10Hand) GetComponents(componentType device.ComponentType) []device.Component { + h.mutex.RLock() + defer h.mutex.RUnlock() + + if components, exists := h.components[componentType]; exists { + result := make([]device.Component, len(components)) + 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 +} + +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) + } + + h.status.IsConnected = true + h.status.IsActive = true + h.status.LastUpdate = time.Now() + + return nil +} + +func (h *L10Hand) Disconnect() error { + h.mutex.Lock() + defer h.mutex.Unlock() + + h.status.IsConnected = false + h.status.IsActive = false + h.status.LastUpdate = time.Now() + + return nil +}