diff --git a/.gitignore b/.gitignore index f9805a6..9080b3a 100644 --- a/.gitignore +++ b/.gitignore @@ -16,6 +16,7 @@ htmlcov/ .ruff_cache/ # Runtime and calibration scratch files +/logs/ *.tmp *.log diff --git a/src/gui_control/gui_control/__init__.py b/src/gui_control/gui_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py new file mode 100644 index 0000000..952ac15 --- /dev/null +++ b/src/gui_control/gui_control/config/constants.py @@ -0,0 +1,262 @@ +# hand_config_const.py +from typing import Dict, List, Optional +from dataclasses import dataclass, field +from types import MappingProxyType + +@dataclass(frozen=True) # frozen=True 让实例真正只读 +class HandConfig: + joint_names: List[str] = field(default_factory=list) + joint_names_en: Optional[List[str]] = None + init_pos: List[int] = field(default_factory=list) + preset_actions: Optional[Dict[str, List[int]]] = None + +# ------------------------------------------------------------------ +# 常量字典（仅构建一次） +# ------------------------------------------------------------------ +_HAND_CONFIGS: Dict[str, HandConfig] = { + "L25": HandConfig( + joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部", + "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆", + "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "食指中部", + "中指中部", "无名指中部", "小拇指中部", "大拇指指尖", "食指指尖", + "中指指尖", "无名指指尖", "小拇指指尖"], + init_pos=[255] * 25, + preset_actions={ + "握拳": [0] * 25, + "张开": [255] * 25, + "OK": [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 0, 0, 255, 255, + 0, 0, 0, 255, 255] + } + ), + "L21": HandConfig( + joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部", + "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆", + "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "预留", + "预留", "预留", "预留", "大拇指指尖", "食指指尖", "中指指尖", + "无名指指尖", "小拇指指尖"], + init_pos=[255] * 25 + ), + "L20": HandConfig( + joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部", + "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆", + "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端", + "中指末端", "无名指末端", "小指末端"], + init_pos=[255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + preset_actions={ + "握拳": [40, 0, 0, 0, 0, 131, 10, 100, 180, 240, 19, 255, 255, 255, 255, 135, 0, 0, 0, 0], + "张开": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "OK": [191, 95, 255, 255, 255, 136, 107, 100, 180, 240, 72, 255, 255, 255, 255, 116, 99, 255, 255, 255], + "点赞": [255, 0, 0, 0, 0, 127, 10, 100, 180, 240, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0], + "拇指对食指": [0, 0, 255, 255, 255, 186, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 183, 255, 255, 255], + "拇指对中指": [0, 255, 0, 255, 255, 145, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 202, 255, 255], + "拇指对无名指": [0, 255, 255, 0, 255, 108, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "拇指对小指": [0, 255, 255, 255, 0, 70, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "准备1": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "壹": [40, 255, 0, 0, 0, 131, 125, 100, 180, 240, 19, 255, 255, 255, 255, 135, 255, 0, 0, 0], + "贰": [40, 255, 255, 0, 0, 81, 35, 177, 180, 240, 19, 255, 255, 255, 255, 135, 255, 255, 0, 0], + "叁": [40, 255, 255, 255, 0, 161, 62, 123, 180, 240, 13, 255, 255, 255, 255, 0, 255, 255, 255, 0], + "肆": [40, 255, 255, 255, 255, 161, 62, 123, 180, 242, 13, 255, 255, 255, 255, 0, 255, 255, 255, 255], + "伍": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "陆": [255, 0, 0, 0, 255, 220, 10, 100, 180, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 255], + "漆": [0, 0, 0, 0, 0, 161, 10, 127, 180, 219, 18, 255, 255, 255, 255, 255, 195, 205, 0, 0], + "捌": [255, 255, 0, 0, 0, 202, 104, 100, 180, 240, 233, 255, 255, 255, 255, 255, 255, 0, 0, 0], + "玖": [40, 255, 0, 0, 0, 131, 103, 100, 180, 240, 19, 255, 255, 255, 255, 135, 47, 0, 0, 0], + + } + ), + "G20": HandConfig( + joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部", + "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆", + "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端", + "中指末端", "无名指末端", "小指末端"], + init_pos=[255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + preset_actions={ + "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0], + "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0], + "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255], + "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255], + "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255], + "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100], + "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0], + "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0], + "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0], + "叁": [96, 255, 255, 255, 0, 0, 200, 132, 76, 80, 68, 255, 255, 255, 255, 144, 255, 255, 255, 0], + "肆": [80, 255, 255, 255, 255, 78, 200, 132, 114, 48, 129, 255, 255, 255, 255, 64, 255, 255, 255, 255], + "伍": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "陆": [255, 0, 0, 0, 255, 255, 156, 126, 125, 42, 245, 255, 255, 255, 255, 255, 0, 0, 0, 255], + "漆": [38, 0, 0, 0, 0, 55, 156, 126, 125, 117, 145, 255, 255, 255, 255, 255, 164, 163, 0, 0], + "捌": [255, 255, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 255, 0, 0, 0], + "玖": [67, 255, 0, 0, 0, 37, 162, 162, 144, 100, 85, 255, 255, 255, 255, 169, 0, 0, 0, 0], + "动作1": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作3": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作4": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作5": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作6": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作7": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作8": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0], + "末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0], + "末端4": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255], + "默认": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255] + + + } + ), + # 大拇指关节球版L10 + # "L10": HandConfig( + # joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", + # "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_yaw"], + # joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部", + # "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"], + # init_pos=[255] * 10, + # preset_actions={ + # "握拳": [75, 128, 0, 0, 0, 0, 128, 128, 128, 57], + # "张开": [255, 128, 255, 255, 255, 255, 128, 128, 128, 128], + # "OK": [110, 128, 75, 255, 255, 255, 128, 128, 128, 68], + # "点赞": [255, 145, 0, 0, 0, 0, 0, 255, 255, 65] + # } + # ), + # 大拇指关节齿轮版L10 + "L10": HandConfig( + joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", + "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"], + joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部", + "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"], + init_pos=[255] * 10, + preset_actions={ + "张开": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255], + "点赞": [255, 255, 0, 0, 0, 0, 128, 67, 89, 255], + "握拳": [90, 0, 0, 0, 0, 0, 128, 67, 89, 197], + "壹": [55, 0, 255, 0, 0, 0, 128, 67, 89, 124], + "贰": [55, 0, 255, 255, 0, 0, 128, 67, 89, 124], + "叁": [116, 255, 255, 255, 255, 0, 128, 67, 89, 255], + "肆": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255], + "伍": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255], + "陆": [255, 255, 0, 0, 0, 255, 128, 67, 89, 255], + "柒1": [255, 37, 119, 112, 0, 0, 128, 67, 89, 211], + "柒2": [91, 37, 119, 112, 0, 0, 128, 67, 89, 211], + "捌": [255, 255, 255, 0, 0, 0, 128, 67, 89, 255], + "玖": [59, 0, 134, 0, 0, 0, 128, 67, 89, 153], + "侧摆0": [255, 0, 255, 255, 255, 255, 128, 67, 89, 153], + "侧摆1": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255], + "侧摆2": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255], + "侧摆3": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255], + "侧摆4": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255], + "侧摆5": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255], + "侧摆6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255], + "弯曲1": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255], + "弯曲2": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255], + "弯曲3": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255], + "弯曲4": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255], + "弯曲5": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255], + "弯曲6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255], + "OK": [84, 39, 122, 255, 255, 255, 128, 67, 89, 255], + "拇指压感1": [134, 39, 99, 255, 255, 255, 128, 67, 89, 255], + "拇指压感2": [73, 39, 91, 255, 255, 255, 128, 67, 89, 255], + "食指压感1": [151, 39, 190, 255, 255, 255, 128, 67, 89, 255], + "食指压感2": [58, 39, 103, 255, 255, 255, 128, 67, 89, 255], + "中指压感1": [40, 39, 255, 128, 255, 255, 128, 67, 89, 209], + "中指压感2": [40, 39, 255, 89, 255, 255, 128, 67, 89, 209], + "无名指压感1": [51, 39, 255, 255, 139, 255, 128, 67, 89, 154], + "无名指压感2": [51, 39, 255, 255, 83, 255, 128, 67, 89, 154], + "小拇指压感1": [62, 39, 255, 255, 255, 155, 128, 67, 89, 101], + "小拇指压感2": [62, 39, 255, 255, 255, 75, 128, 67, 89, 101], + } + ), + # 大拇指关节球版L7 + # "L7": HandConfig( + # joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", + # "小拇指弯曲", "拇指旋转"], + # init_pos=[250] * 7, + # preset_actions={ + # "点赞": [255, 111, 0, 0, 0, 0, 86], + # "握拳": [71, 79, 0, 0, 0, 0, 64], + # "张开": [255, 111, 250, 250, 250, 250, 55], + # "OK": [141, 111, 168, 250, 250, 250, 86], + + # } + # ), + # 大拇指关节齿轮版L7 + "L7": HandConfig( + joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"], + joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲", "拇指旋转"], + init_pos=[250] * 7, + preset_actions={ + "张开": [255, 111, 250, 250, 250, 250, 55], + "点赞": [255, 255, 0, 0, 0, 0, 255], + "赞2": [255, 0, 0, 0, 0, 0, 255], + "握拳": [65, 0, 0, 0, 0, 0, 93], + "壹": [66, 0, 255, 0, 0, 0, 93], + "壹1": [61, 0, 255, 0, 0, 0, 255], + "贰": [0, 0, 255, 255, 0, 0, 255], + "叁": [0, 0, 255, 255, 255, 0, 255], + "肆": [0, 0, 255, 255, 255, 255, 119], + "伍": [255, 111, 250, 250, 250, 250, 55], + "OK": [99, 15, 146, 250, 250, 250, 206], + "拇指压感1": [99, 15, 206, 250, 250, 250, 206], + "拇指压感2": [109, 15, 70, 250, 250, 250, 206], + "食指压感1": [99, 15, 206, 250, 250, 250, 206], + "食指压感2": [69, 15, 140, 250, 250, 250, 206], + "中指压感": [82, 15, 255, 136, 250, 250, 170], + "无名指压感": [70, 15, 255, 255, 141, 250, 125], + "小拇指压感": [70, 15, 255, 255, 255, 120, 78], + "准备1": [70, 15, 255, 255, 255, 255, 78] + + } + ), + "O6": HandConfig( + joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"], + joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"], + init_pos=[250] * 6, + preset_actions={ + "张开": [250, 250, 250, 250, 250, 250], + "壹": [125, 18, 255, 0, 0, 0], + "贰": [92, 87, 255, 255, 0, 0], + "叁": [92, 87, 255, 255, 255, 0], + "肆": [92, 87, 255, 255, 255, 255], + "伍": [255, 255, 255, 255, 255, 255], + "OK": [139, 91, 103, 250, 250, 250], + "点赞": [250, 79, 0, 0, 0, 0], + "握拳": [102, 18, 0, 0, 0, 0], + } + ), + "L6": HandConfig( + joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"], + joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"], + init_pos=[250] * 6, + preset_actions={ + "张开": [250, 250, 250, 250, 250, 250], + "壹": [0, 18, 255, 0, 0, 0], + "贰": [0, 39, 255, 255, 0, 0], + "叁": [0, 39, 255, 255, 255, 0], + "肆": [0, 0, 255, 255, 255, 255], + "伍": [255, 255, 255, 255, 255, 255], + "OK": [74, 13, 153, 255, 255, 255], + "点赞": [255, 255, 0, 0, 0, 0], + "握拳": [79, 11, 0, 0, 0, 0], + "序列动作1": [250, 250, 250, 250, 250, 250], + "序列动作2": [250, 250, 0, 250, 250, 0], + "序列动作3": [250, 250, 0, 0, 0, 0], + "序列动作4": [250, 250, 0, 0, 0, 255], + "序列动作5": [250, 250, 0, 0, 255, 255], + "序列动作6": [250, 250, 0, 255, 255, 255], + "序列动作7": [250, 250, 250, 250, 250, 250], + "食指压感准备1": [0, 18, 255, 0, 0, 0], + "食指压感测试": [9, 42, 55, 250, 250, 250], + "食指压感准备2": [0, 18, 255, 0, 0, 0], + "拇指压感准备1": [139, 18, 130, 0, 0, 0], + "拇指压感测试": [39, 30, 122, 250, 250, 250], + "拇指压感准备2": [139, 18, 130, 0, 0, 0] + } + ), +} +HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS) diff --git a/src/gui_control/gui_control/gui_control.py b/src/gui_control/gui_control/gui_control.py new file mode 100644 index 0000000..7b7bef6 --- /dev/null +++ b/src/gui_control/gui_control/gui_control.py @@ -0,0 +1,808 @@ +import sys +import time, json +import threading +from dataclasses import dataclass +from typing import List, Dict +import rclpy +from rclpy.node import Node +from std_msgs.msg import String, Header +from sensor_msgs.msg import JointState +from PyQt5.QtCore import Qt, pyqtSignal, QTimer, QObject, QEvent +from PyQt5.QtWidgets import ( + QApplication, QWidget, QVBoxLayout, QHBoxLayout, QGridLayout, + QSlider, QLabel, QPushButton, QGroupBox, QScrollArea, QTabWidget, + QFrame, QSplitter, QMessageBox, QTextEdit +) +from PyQt5.QtGui import QFont + +from .utils.mapping import * + +from .config.constants import _HAND_CONFIGS +LOOP_TIME = 1000 # 循环动作间隔时间 毫秒 +class ROS2NodeManager(QObject): + """ROS2节点管理器，处理ROS通信""" + status_updated = pyqtSignal(str, str) # 状态类型, 消息内容 + + def __init__(self, node_name: str = "hand_control_node"): + super().__init__() + self.node = None + self.publisher = None + self.joint_state = JointState() + self.joint_state.header = Header() + + # 初始化ROS2节点 + self.init_node(node_name) + + def init_node(self, node_name: str): + """初始化ROS2节点""" + try: + if not rclpy.ok(): + rclpy.init(args=None) + self.node = Node(node_name) + + # 声明参数 + self.node.declare_parameter('hand_type', 'right') + self.node.declare_parameter('hand_joint', 'L10') + self.node.declare_parameter('topic_hz', 30) + self.node.declare_parameter('is_arc', False) + + # 获取参数 + self.hand_type = self.node.get_parameter('hand_type').value + self.hand_joint = self.node.get_parameter('hand_joint').value + self.hz = self.node.get_parameter('topic_hz').value + self.is_arc = self.node.get_parameter('is_arc').value + + if self.is_arc == True: + # 创建发布者 + self.publisher_arc = self.node.create_publisher( + JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10 + ) + # 创建发布者 + self.publisher = self.node.create_publisher( + JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10 + ) + # 新增 speed / torque 发布者 + self.speed_pub = self.node.create_publisher( + String, f'/cb_hand_setting_cmd', 10) + self.torque_pub = self.node.create_publisher( + String, f'/cb_hand_setting_cmd', 10) + self.status_updated.emit("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}") + + # 启动ROS2自旋线程 + self.spin_thread = threading.Thread(target=self.spin_node, daemon=True) + self.spin_thread.start() + except Exception as e: + self.status_updated.emit("error", f"ROS2初始化失败: {str(e)}") + raise + + def spin_node(self): + """运行ROS2节点自旋循环""" + while rclpy.ok() and self.node: + rclpy.spin_once(self.node, timeout_sec=0.1) + + def publish_joint_state(self, positions: List[int]): + """发布关节状态消息""" + if not self.publisher or not self.node: + self.status_updated.emit("error", "ROS2发布者未初始化") + return + + try: + self.joint_state.header.stamp = self.node.get_clock().now().to_msg() + self.joint_state.position = [float(pos) for pos in positions] + # self.joint_state.velocity = [0.1] * len(positions) + # self.joint_state.effort = [0.01] * len(positions) + # 如果有关节名称，添加到消息中 + #hand_config = HandConfig.from_hand_type(self.hand_joint) + hand_config = _HAND_CONFIGS[self.hand_joint] + if len(hand_config.joint_names) == len(positions): + if hand_config.joint_names_en != None: + self.joint_state.name = hand_config.joint_names_en + else: + self.joint_state.name = hand_config.joint_names + + self.publisher.publish(self.joint_state) + if self.is_arc == True: + if self.hand_joint == "O6": + if self.hand_type == "left": + pose = range_to_arc_left(positions,self.hand_joint) + elif self.hand_type == "right": + pose = range_to_arc_right(positions,self.hand_joint) + elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25": + if self.hand_type == "left": + pose = range_to_arc_left(positions,self.hand_joint) + elif self.hand_type == "right": + pose = range_to_arc_right(positions,self.hand_joint) + elif self.hand_joint == "L10": + if self.hand_type == "left": + pose = range_to_arc_left_10(positions) + elif self.hand_type == "right": + pose = range_to_arc_right_10(positions) + elif self.hand_joint == "L20": + if self.hand_type == "left": + pose = range_to_arc_left_l20(positions) + elif self.hand_type == "right": + pose = range_to_arc_right_l20(positions) + else: + #print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True) + pass + self.joint_state.position = [float(pos) for pos in pose] + self.publisher_arc.publish(self.joint_state) + self.status_updated.emit("info", "关节状态已发布") + except Exception as e: + self.status_updated.emit("error", f"发布失败: {str(e)}") + + def publish_speed(self, val: int): + joint_len = 0 + if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"): + joint_len = 6 + elif self.hand_joint == "L7": + joint_len = 7 + elif self.hand_joint == "L10": + joint_len = 10 + else: + joint_len = 5 + msg = String() + v = [val] * joint_len + data = { + "setting_cmd": "set_speed", + "params": {"hand_type":self.hand_type,"speed": v}, + } + msg.data = json.dumps(data) + print(f"速度值：{v}", flush=True) + self.speed_pub.publish(msg) + + def publish_torque(self, val: int): + joint_len = 0 + if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"): + joint_len = 6 + elif self.hand_joint == "L7": + joint_len = 7 + elif self.hand_joint == "L10": + joint_len = 10 + else: + joint_len = 5 + msg = String() + v = [val] * joint_len + data = { + "setting_cmd": "set_max_torque_limits", + "params": {"hand_type":self.hand_type,"torque": v}, + } + + msg.data = json.dumps(data) + print(f"扭矩值：{v}", flush=True) + self.torque_pub.publish(msg) + + def shutdown(self): + """关闭ROS2节点""" + if self.node: + self.node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +class HandControlGUI(QWidget): + """灵巧手控制界面""" + status_updated = pyqtSignal(str, str) # 状态类型, 消息内容 + + def __init__(self, ros_manager: ROS2NodeManager): + super().__init__() + + # 循环控制变量 + self.cycle_timer = None # 循环定时器 + self.current_action_index = -1 # 当前动作索引 + self.preset_buttons = [] # 存储预设动作按钮引用 + + # 设置ROS管理器 + self.ros_manager = ros_manager + self.ros_manager.status_updated.connect(self.update_status) + + # 获取手部配置 + self.hand_joint = self.ros_manager.hand_joint + self.hand_type = self.ros_manager.hand_type + self.hand_config = _HAND_CONFIGS[self.hand_joint] + + # 初始化UI + self.init_ui() + + # 设置定时器发布关节状态 + self.publish_timer = QTimer(self) + self.publish_timer.setInterval(int(1000 / self.ros_manager.hz)) + self.publish_timer.timeout.connect(self.publish_joint_state) + self.publish_timer.start() + + def init_ui(self): + """初始化用户界面""" + # 设置窗口属性 + self.setWindowTitle(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}') + self.setMinimumSize(1200, 900) + + # 设置样式 + self.setStyleSheet(""" + QWidget { + font-family: 'Microsoft YaHei', 'SimHei', sans-serif; + font-size: 12px; + } + QGroupBox { + border: 1px solid #CCCCCC; + border-radius: 6px; + margin-top: 6px; + padding: 10px; + } + QGroupBox::title { + subcontrol-origin: margin; + left: 10px; + padding: 0 5px 0 5px; + color: #165DFF; + font-weight: bold; + } + QSlider::groove:horizontal { + border: 1px solid #999999; + height: 8px; + border-radius: 4px; + background: #CCCCCC; + margin: 2px 0; + } + QSlider::handle:horizontal { + background: qlineargradient(x1:0, y1:0, x2:1, y2:1, stop:0 #165DFF, stop:1 #0E42D2); + border: 1px solid #5C8AFF; + width: 18px; + margin: -5px 0; + border-radius: 9px; + } + QPushButton { + background-color: #E0E0E0; + border: 1px solid #CCCCCC; + border-radius: 4px; + padding: 5px 10px; + min-width: 80px; + } + QPushButton:hover { + background-color: #F0F0F0; + } + QPushButton:pressed { + background-color: #D0D0D0; + } + QPushButton[category="preset"] { + background-color: #E6F7FF; + color: #1890FF; + border-color: #91D5FF; + } + QPushButton[category="preset"]:hover { + background-color: #B3E0FF; + } + QPushButton[category="action"] { + background-color: #FFF7E6; + color: #FA8C16; + border-color: #FFD591; + } + QPushButton[category="action"]:hover { + background-color: #FFE6B3; + } + QPushButton[category="danger"] { + background-color: #FFF1F0; + color: #F5222D; + border-color: #FFCCC7; + } + QPushButton[category="danger"]:hover { + background-color: #FFE8E6; + } + QLabel#StatusLabel { + padding: 5px; + border-radius: 4px; + } + QLabel#StatusInfo { + background-color: #F0F7FF; + color: #0066CC; + } + QLabel#StatusError { + background-color: #FFF0F0; + color: #CC0000; + } + /* 数值显示面板样式 */ + QTextEdit#ValueDisplay { + background-color: #F8F8F8; + border: 1px solid #CCCCCC; + border-radius: 4px; + padding: 10px; + font-family: Consolas, monospace; + font-size: 12px; + } + """) + + # 创建主垂直布局 + main_layout = QVBoxLayout(self) + + # 创建水平分割器（原有三个面板） + splitter = QSplitter(Qt.Horizontal) + + # 创建左侧关节控制面板 + self.joint_control_panel = self.create_joint_control_panel() + splitter.addWidget(self.joint_control_panel) + + # 创建中间预设动作面板 + self.preset_actions_panel = self.create_preset_actions_panel() + splitter.addWidget(self.preset_actions_panel) + + # 创建右侧状态监控面板 + self.status_monitor_panel = self.create_status_monitor_panel() + splitter.addWidget(self.status_monitor_panel) + + # 设置分割器比例 + splitter.setSizes([500, 300, 400]) + + # 添加分割器到主布局，并设置拉伸因子为1（可伸缩） + main_layout.addWidget(splitter, stretch=1) + + # 创建并添加数值显示面板，设置拉伸因子为0（不可伸缩） + self.value_display_panel = self.create_value_display_panel() + main_layout.addWidget(self.value_display_panel, stretch=0) + + # 初始更新数值显示 + self.update_value_display() + + def create_joint_control_panel(self): + """创建关节控制面板""" + panel = QWidget() + layout = QVBoxLayout(panel) + + # 创建标题 + title_label = QLabel(f"关节控制 - {self.hand_joint}") + title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold)) + layout.addWidget(title_label) + + # 创建滑动条滚动区域 + scroll_area = QScrollArea() + scroll_area.setWidgetResizable(True) + scroll_area.setFrameShape(QFrame.NoFrame) + + scroll_content = QWidget() + self.sliders_layout = QGridLayout(scroll_content) + self.sliders_layout.setSpacing(10) + + # 创建滑动条 + self.create_joint_sliders() + + scroll_area.setWidget(scroll_content) + layout.addWidget(scroll_area) + + return panel + + + + def create_joint_sliders(self): + """创建关节滑动条""" + # 清除现有滑动条 + for i in reversed(range(self.sliders_layout.count())): + item = self.sliders_layout.itemAt(i) + if item.widget(): + item.widget().deleteLater() + + # 创建新滑动条 + self.sliders = [] + self.slider_labels = [] + + for i, (name, value) in enumerate(zip( + self.hand_config.joint_names, self.hand_config.init_pos + )): + # 创建标签 + label = QLabel(f"{name}: {value}") + label.setMinimumWidth(120) + + # 创建滑动条 + slider = QSlider(Qt.Horizontal) + slider.setRange(0, 255) + slider.setValue(value) + slider.valueChanged.connect( + lambda val, idx=i: self.on_slider_value_changed(idx, val) + ) + + # 添加到布局 + row, col = divmod(i, 1) + self.sliders_layout.addWidget(label, row, 0) + self.sliders_layout.addWidget(slider, row, 1) + + self.sliders.append(slider) + self.slider_labels.append(label) + + def create_preset_actions_panel(self): + """创建预设动作面板""" + panel = QWidget() + layout = QVBoxLayout(panel) + + # 自定义预设动作 + sys_preset_group = QGroupBox("自定义预设动作(名称不可重复)") + sys_preset_layout = QGridLayout(sys_preset_group) + sys_preset_layout.setSpacing(8) + + # 添加系统预设动作按钮 + self.create_system_preset_buttons(sys_preset_layout) + layout.addWidget(sys_preset_group) + + # 添加动作按钮 + actions_layout = QHBoxLayout() + + # 添加循环运行按钮 + self.cycle_button = QPushButton("循环预设动作") + self.cycle_button.setProperty("category", "action") + self.cycle_button.clicked.connect(self.on_cycle_clicked) + actions_layout.addWidget(self.cycle_button) + + self.home_button = QPushButton("回到初始位置") + self.home_button.setProperty("category", "action") + self.home_button.clicked.connect(self.on_home_clicked) + actions_layout.addWidget(self.home_button) + + self.stop_button = QPushButton("停止所有动作") + self.stop_button.setProperty("category", "danger") + self.stop_button.clicked.connect(self.on_stop_clicked) + actions_layout.addWidget(self.stop_button) + + layout.addLayout(actions_layout) + + return panel + + def create_system_preset_buttons(self, parent_layout): + """创建系统预设动作按钮""" + self.preset_buttons = [] # 清空按钮列表 + if self.hand_config.preset_actions: + buttons = [] + for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()): + button = QPushButton(name) + button.setProperty("category", "preset") + button.clicked.connect( + lambda checked, pos=positions: self.on_preset_action_clicked(pos) + ) + buttons.append(button) + self.preset_buttons.append(button) # 保存按钮引用 + + # 添加到网格布局 + cols = 2 + for i, button in enumerate(buttons): + row, col = divmod(i, cols) + parent_layout.addWidget(button, row, col) + + def create_status_monitor_panel(self): + """创建状态监控面板（速度/扭矩各占一行，并实时显示滑块值）""" + panel = QWidget() + layout = QVBoxLayout(panel) + + # —— 1. 标题 —— + title_label = QLabel("状态监控") + title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold)) + layout.addWidget(title_label) + + # —— 2. 新增：速度与扭矩设置（每行一个）—— + quick_set_gb = QGroupBox("快速设置") + qv_layout = QVBoxLayout(quick_set_gb) + + # 速度行 + speed_hbox = QHBoxLayout() + speed_hbox.addWidget(QLabel("速度:")) + self.speed_slider = QSlider(Qt.Horizontal) + self.speed_slider.setRange(0, 255) + self.speed_slider.setValue(255) + self.speed_slider.setMinimumWidth(150) + speed_hbox.addWidget(self.speed_slider) + self.speed_val_lbl = QLabel("255") # 实时值 + self.speed_val_lbl.setMinimumWidth(30) + speed_hbox.addWidget(self.speed_val_lbl) + self.speed_btn = QPushButton("设置速度") + self.speed_btn.clicked.connect( + lambda: ( + self.ros_manager.publish_speed(self.speed_slider.value()), + self.status_updated.emit( + "info", f"速度已设为 {self.speed_slider.value()}") + )) + speed_hbox.addWidget(self.speed_btn) + speed_hbox.addStretch() + qv_layout.addLayout(speed_hbox) + + # 扭矩行 + torque_hbox = QHBoxLayout() + torque_hbox.addWidget(QLabel("扭矩:")) + self.torque_slider = QSlider(Qt.Horizontal) + self.torque_slider.setRange(0, 255) + self.torque_slider.setValue(255) + self.torque_slider.setMinimumWidth(150) + torque_hbox.addWidget(self.torque_slider) + self.torque_val_lbl = QLabel("255") + self.torque_val_lbl.setMinimumWidth(30) + torque_hbox.addWidget(self.torque_val_lbl) + self.torque_btn = QPushButton("设置扭矩") + self.torque_btn.clicked.connect( + lambda: ( + self.ros_manager.publish_torque(self.torque_slider.value()), + self.status_updated.emit( + "info", f"扭矩已设为 {self.torque_slider.value()}") + )) + torque_hbox.addWidget(self.torque_btn) + torque_hbox.addStretch() + qv_layout.addLayout(torque_hbox) + + layout.addWidget(quick_set_gb) + + # —— 3. 原有标签页部分，完全不动 —— + tab_widget = QTabWidget() + + # 系统信息标签页 + sys_info_widget = QWidget() + sys_info_layout = QVBoxLayout(sys_info_widget) + + conn_group = QGroupBox("连接状态") + conn_layout = QVBoxLayout(conn_group) + if self.ros_manager.publisher.get_subscription_count() > 0: + self.connection_status = QLabel("ROS2节点已连接") + self.connection_status.setObjectName("StatusLabel") + self.connection_status.setObjectName("StatusInfo") + else: + self.connection_status = QLabel("ROS2节点未连接") + self.connection_status.setObjectName("StatusLabel") + self.connection_status.setObjectName("StatusError") + conn_layout.addWidget(self.connection_status) + + hand_info_group = QGroupBox("手部信息") + hand_info_layout = QVBoxLayout(hand_info_group) + info_text = f"""手部类型: {self.hand_type} +关节型号: {self.hand_joint} +关节数量: {len(self.hand_config.joint_names)} +发布频率: {self.ros_manager.hz} Hz""" + self.hand_info_label = QLabel(info_text) + self.hand_info_label.setWordWrap(True) + hand_info_layout.addWidget(self.hand_info_label) + + sys_info_layout.addWidget(conn_group) + sys_info_layout.addWidget(hand_info_group) + sys_info_layout.addStretch() + tab_widget.addTab(sys_info_widget, "系统信息") + + # 状态日志标签页 + log_widget = QWidget() + log_layout = QVBoxLayout(log_widget) + self.status_log = QLabel("等待系统启动...") + self.status_log.setObjectName("StatusLabel") + self.status_log.setObjectName("StatusInfo") + self.status_log.setWordWrap(True) + self.status_log.setMinimumHeight(300) + log_layout.addWidget(self.status_log) + clear_log_btn = QPushButton("清除日志") + clear_log_btn.clicked.connect(self.clear_status_log) + log_layout.addWidget(clear_log_btn) + tab_widget.addTab(log_widget, "状态日志") + + layout.addWidget(tab_widget) + + # —— 4. 实时更新滑块值 —— + self.speed_slider.valueChanged.connect( + lambda v: self.speed_val_lbl.setText(str(v))) + self.torque_slider.valueChanged.connect( + lambda v: self.torque_val_lbl.setText(str(v))) + return panel + + def create_value_display_panel(self): + """创建滑动条数值显示面板""" + panel = QGroupBox("关节数值列表") + layout = QVBoxLayout(panel) + + # 设置布局上下间隔为20像素 + layout.setContentsMargins(10, 20, 10, 20) + + self.value_display = QTextEdit() + self.value_display.setObjectName("ValueDisplay") + self.value_display.setReadOnly(True) # 设置只读模式，允许复制 + self.value_display.setMinimumHeight(60) # 调整最小高度 + self.value_display.setMaximumHeight(80) # 限制最大高度 + self.value_display.setText("[]") + + layout.addWidget(self.value_display) + + return panel + + def on_slider_value_changed(self, index: int, value: int): + """滑动条值改变事件处理""" + if 0 <= index < len(self.slider_labels): + joint_name = self.hand_config.joint_names[index] + self.slider_labels[index].setText(f"{joint_name}: {value}") + + # 更新数值显示 + self.update_value_display() + + def update_value_display(self): + """更新数值显示面板内容""" + # 获取所有滑动条的当前值 + values = [slider.value() for slider in self.sliders] + + # 格式化显示为列表形式 + self.value_display.setText(f"{values}") + + def on_preset_action_clicked(self, positions: List[int]): + """预设动作按钮点击事件处理""" + if len(positions) != len(self.sliders): + QMessageBox.warning( + self, "动作不匹配", + f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配" + ) + return + + # 更新滑动条 + for i, (slider, pos) in enumerate(zip(self.sliders, positions)): + slider.setValue(pos) + self.on_slider_value_changed(i, pos) + + # 发布关节状态 + self.publish_joint_state() + + def on_home_clicked(self): + """回到初始位置按钮点击事件处理""" + for slider, pos in zip(self.sliders, self.hand_config.init_pos): + slider.setValue(pos) + + self.publish_joint_state() + self.status_updated.emit("info", "回到初始位置") + + # 更新数值显示 + self.update_value_display() + + def on_stop_clicked(self): + """停止所有动作按钮点击事件处理""" + # 停止循环定时器 + if self.cycle_timer and self.cycle_timer.isActive(): + self.cycle_timer.stop() + self.cycle_timer = None + self.cycle_button.setText("循环运行预设动作") + self.reset_preset_buttons_color() + + self.status_updated.emit("warning", "已停止所有动作") + + def on_cycle_clicked(self): + """循环运行预设动作按钮点击事件处理""" + if not self.hand_config.preset_actions: + QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行") + return + + if self.cycle_timer and self.cycle_timer.isActive(): + # 停止循环 + self.cycle_timer.stop() + self.cycle_timer = None + self.cycle_button.setText("循环运行预设动作") + self.reset_preset_buttons_color() + self.status_updated.emit("info", "已停止循环运行预设动作") + else: + # 开始循环 + self.current_action_index = -1 # 重置索引 + self.cycle_timer = QTimer(self) + self.cycle_timer.timeout.connect(self.run_next_action) + self.cycle_timer.start(LOOP_TIME) # 2秒间隔 + self.cycle_button.setText("停止循环运行") + self.status_updated.emit("info", "开始循环运行预设动作") + self.run_next_action() # 立即运行第一个动作 + + def run_next_action(self): + """运行下一个预设动作""" + if not self.hand_config.preset_actions: + return + + # 重置所有按钮颜色 + self.reset_preset_buttons_color() + + # 计算下一个动作索引 + self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions) + + # 获取下一个动作 + action_names = list(self.hand_config.preset_actions.keys()) + action_name = action_names[self.current_action_index] + action_positions = self.hand_config.preset_actions[action_name] + + # 执行动作 + self.on_preset_action_clicked(action_positions) + + # 高亮当前动作按钮 + if 0 <= self.current_action_index < len(self.preset_buttons): + button = self.preset_buttons[self.current_action_index] + button.setStyleSheet("background-color: green; color: white; border-color: #91D5FF;") + + self.status_updated.emit("info", f"运行预设动作: {action_name}") + + def reset_preset_buttons_color(self): + """重置所有预设按钮颜色""" + for button in self.preset_buttons: + button.setStyleSheet("") # 恢复默认样式 + button.setProperty("category", "preset") # 恢复类别属性 + # 强制样式刷新 + button.style().unpolish(button) + button.style().polish(button) + + def on_joint_type_changed(self, joint_type: str): + """关节类型改变事件处理""" + self.hand_joint = joint_type + self.hand_config = _HAND_CONFIGS[self.hand_joint] + + # 更新手部信息 + info_text = f"""手部类型: {self.hand_type} +关节型号: {self.hand_joint} +关节数量: {len(self.hand_config.joint_names)} +发布频率: {self.ros_manager.hz} Hz""" + self.hand_info_label.setText(info_text) + + # 重新创建滑动条和预设按钮 + self.create_joint_sliders() + self.create_system_preset_buttons(self.sys_preset_layout) # 假设sys_preset_layout是类变量 + + # 更新数值显示 + self.update_value_display() + self.status_updated.emit("info", f"已切换到手部型号: {joint_type}") + + def publish_joint_state(self): + """发布当前关节状态""" + positions = [slider.value() for slider in self.sliders] + self.ros_manager.publish_joint_state(positions) + + def update_status(self, status_type: str, message: str): + """更新状态显示""" + # 更新连接状态 + if status_type == "info" and "ROS2节点初始化成功" in message: + self.connection_status.setText("ROS2节点已连接") + self.connection_status.setObjectName("StatusLabel") + self.connection_status.setObjectName("StatusInfo") + + # 更新日志 + current_time = time.strftime("%H:%M:%S") + log_entry = f"[{current_time}] {message}\n" + current_log = self.status_log.text() + + if len(current_log) > 10000: # 限制日志长度 + current_log = current_log[-10000:] + + self.status_log.setText(log_entry + current_log) + + # 设置日志样式 + self.status_log.setObjectName("StatusLabel") + if status_type == "error": + self.status_log.setObjectName("StatusError") + else: + self.status_log.setObjectName("StatusInfo") + + def clear_status_log(self): + """清除状态日志""" + self.status_log.setText("日志已清除") + self.status_log.setObjectName("StatusLabel") + self.status_log.setObjectName("StatusInfo") + + def closeEvent(self, event): + """窗口关闭事件处理""" + if self.cycle_timer and self.cycle_timer.isActive(): + self.cycle_timer.stop() + super().closeEvent(event) + +def main(args=None): + """主函数""" + try: + # 创建ROS2节点管理器 + ros_manager = ROS2NodeManager() + + # 创建Qt应用 + app = QApplication(sys.argv) + + # 创建GUI + window = HandControlGUI(ros_manager) + + # 连接状态更新信号 + ros_manager.status_updated.connect(window.update_status) + window.status_updated = ros_manager.status_updated + + # 显示窗口 + window.show() + + # 运行应用 + exit_code = app.exec_() + + # 清理ROS2 + if rclpy.ok(): + ros_manager.node.destroy_node() + rclpy.shutdown() + + sys.exit(exit_code) + except Exception as e: + print(f"应用程序启动失败: {str(e)}") + sys.exit(1) + +if __name__ == '__main__': + main() diff --git a/src/gui_control/gui_control/gui_control.py.ttk b/src/gui_control/gui_control/gui_control.py.ttk new file mode 100644 index 0000000..aa86c30 --- /dev/null +++ b/src/gui_control/gui_control/gui_control.py.ttk @@ -0,0 +1,790 @@ +import sys +import time +import json +import threading +from dataclasses import dataclass +from typing import List, Dict +import rclpy +from rclpy.node import Node +from std_msgs.msg import String, Header +from sensor_msgs.msg import JointState +import tkinter as tk +from tkinter import ttk, scrolledtext, messagebox +import tkinter.font as tkfont + +from .utils.mapping import * +from .config.constants import _HAND_CONFIGS +LOOP_TIME = 1000 # 循环动作间隔时间 毫秒 +class ROS2NodeManager: + """ROS2节点管理器，处理ROS通信""" + + def __init__(self, node_name: str = "hand_control_node"): + self.node = None + self.publisher = None + self.joint_state = JointState() + self.joint_state.header = Header() + self.status_callbacks = [] + + # 初始化ROS2节点 + self.init_node(node_name) + + def add_status_callback(self, callback): + """添加状态回调函数""" + self.status_callbacks.append(callback) + + def emit_status(self, status_type: str, message: str): + """发射状态信号""" + for callback in self.status_callbacks: + callback(status_type, message) + + def init_node(self, node_name: str): + """初始化ROS2节点""" + try: + if not rclpy.ok(): + rclpy.init(args=None) + self.node = Node(node_name) + + # 声明参数 + self.node.declare_parameter('hand_type', 'left') + self.node.declare_parameter('hand_joint', 'L10') + self.node.declare_parameter('topic_hz', 30) + self.node.declare_parameter('is_arc', False) + + # 获取参数 + self.hand_type = self.node.get_parameter('hand_type').value + self.hand_joint = self.node.get_parameter('hand_joint').value + self.hz = self.node.get_parameter('topic_hz').value + self.is_arc = self.node.get_parameter('is_arc').value + + if self.is_arc == True: + # 创建发布者 + self.publisher_arc = self.node.create_publisher( + JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10 + ) + # 创建发布者 + self.publisher = self.node.create_publisher( + JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10 + ) + # 新增 speed / torque 发布者 + self.speed_pub = self.node.create_publisher( + String, f'/cb_hand_setting_cmd', 10) + self.torque_pub = self.node.create_publisher( + String, f'/cb_hand_setting_cmd', 10) + self.emit_status("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}") + + # 启动ROS2自旋线程 + self.spin_thread = threading.Thread(target=self.spin_node, daemon=True) + self.spin_thread.start() + except Exception as e: + self.emit_status("error", f"ROS2初始化失败: {str(e)}") + raise + + def spin_node(self): + """运行ROS2节点自旋循环""" + while rclpy.ok() and self.node: + rclpy.spin_once(self.node, timeout_sec=0.1) + + def publish_joint_state(self, positions: List[int]): + """发布关节状态消息""" + if not self.publisher or not self.node: + self.emit_status("error", "ROS2发布者未初始化") + return + + try: + self.joint_state.header.stamp = self.node.get_clock().now().to_msg() + self.joint_state.position = [float(pos) for pos in positions] + hand_config = _HAND_CONFIGS[self.hand_joint] + if len(hand_config.joint_names) == len(positions): + if hand_config.joint_names_en != None: + self.joint_state.name = hand_config.joint_names_en + else: + self.joint_state.name = hand_config.joint_names + + self.publisher.publish(self.joint_state) + if self.is_arc == True: + if self.hand_joint == "O6": + if self.hand_type == "left": + pose = range_to_arc_left(positions,self.hand_joint) + elif self.hand_type == "right": + pose = range_to_arc_right(positions,self.hand_joint) + elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25": + if self.hand_type == "left": + pose = range_to_arc_left(positions,self.hand_joint) + elif self.hand_type == "right": + pose = range_to_arc_right(positions,self.hand_joint) + elif self.hand_joint == "L10": + if self.hand_type == "left": + pose = range_to_arc_left_10(positions) + elif self.hand_type == "right": + pose = range_to_arc_right_10(positions) + elif self.hand_joint == "L20": + if self.hand_type == "left": + pose = range_to_arc_left_l20(positions) + elif self.hand_type == "right": + pose = range_to_arc_right_l20(positions) + else: + print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True) + self.joint_state.position = [float(pos) for pos in pose] + self.publisher_arc.publish(self.joint_state) + self.emit_status("info", "关节状态已发布") + except Exception as e: + self.emit_status("error", f"发布失败: {str(e)}") + + def publish_speed(self, val: int): + joint_len = 0 + if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"): + joint_len = 6 + elif self.hand_joint == "L7": + joint_len = 7 + elif self.hand_joint == "L10": + joint_len = 10 + else: + joint_len = 5 + msg = String() + v = [val] * joint_len + data = { + "setting_cmd": "set_speed", + "params": {"hand_type":self.hand_type,"speed": v}, + } + msg.data = json.dumps(data) + print(f"速度值：{v}", flush=True) + self.speed_pub.publish(msg) + + def publish_torque(self, val: int): + joint_len = 0 + if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"): + joint_len = 6 + elif self.hand_joint == "L7": + joint_len = 7 + elif self.hand_joint == "L10": + joint_len = 10 + else: + joint_len = 5 + msg = String() + v = [val] * joint_len + data = { + "setting_cmd": "set_max_torque_limits", + "params": {"hand_type":self.hand_type,"torque": v}, + } + + msg.data = json.dumps(data) + print(f"扭矩值：{v}", flush=True) + self.torque_pub.publish(msg) + + def shutdown(self): + """关闭ROS2节点""" + if self.node: + self.node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +class HandControlGUI: + """灵巧手控制界面""" + + def __init__(self, ros_manager: ROS2NodeManager): + self.ros_manager = ros_manager + self.ros_manager.add_status_callback(self.update_status) + + # 获取手部配置 + self.hand_joint = self.ros_manager.hand_joint + self.hand_type = self.ros_manager.hand_type + self.hand_config = _HAND_CONFIGS[self.hand_joint] + + # 循环控制变量 + self.cycle_timer = None + self.current_action_index = -1 + self.preset_buttons = [] + + # 初始化UI + self.init_ui() + + # 设置定时器发布关节状态 + self.publish_timer_id = None + self.start_publish_timer() + + def init_ui(self): + """初始化用户界面""" + # 创建主窗口 + self.root = tk.Tk() + self.root.title(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}') + self.root.geometry('1300x700') + + # 设置样式 + self.style = ttk.Style() + self.style.configure('TFrame', background='#f0f0f0') + self.style.configure('TLabel', background='#f0f0f0', font=('Microsoft YaHei', 10)) + self.style.configure('Title.TLabel', font=('Microsoft YaHei', 14, 'bold'), foreground='#165DFF') + self.style.configure('Group.TLabelframe', borderwidth=2, relief='groove') + self.style.configure('Group.TLabelframe.Label', font=('Microsoft YaHei', 10, 'bold'), foreground='#165DFF') + # 配置信息框样式 - 灰色背景 + self.style.configure('Info.TFrame', background='#e0e0e0') + # 配置高亮样式 - 滑动时的背景色 + self.style.configure('Highlight.TFrame', background='#e6f7ff') # 浅蓝色背景 + + # 初始化当前点击按钮索引 + self.current_clicked_button = None + + # 创建主框架 + main_frame = ttk.Frame(self.root) + main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10) + + # 创建水平分割的框架 + self.paned_window = ttk.PanedWindow(main_frame, orient=tk.HORIZONTAL) + self.paned_window.pack(fill=tk.BOTH, expand=True) + + # 创建左侧关节控制面板 + self.joint_control_panel = self.create_joint_control_panel() + self.paned_window.add(self.joint_control_panel, weight=5) + + # 创建中间预设动作面板 + self.preset_actions_panel = self.create_preset_actions_panel() + self.paned_window.add(self.preset_actions_panel, weight=3) + + # 创建右侧状态监控面板 + self.status_monitor_panel = self.create_status_monitor_panel() + self.paned_window.add(self.status_monitor_panel, weight=4) + + # 创建底部数值显示面板 + self.value_display_panel = self.create_value_display_panel() + self.value_display_panel.pack(fill=tk.X, pady=(10, 0)) + + # 绑定窗口大小变化事件 + self.joint_control_panel.bind('', self.on_joint_panel_resize) + + # 初始更新数值显示 + self.update_value_display() + + def create_joint_control_panel(self): + """创建关节控制面板""" + frame = ttk.Frame(self.root) + frame.config(width=600) # 设置最小宽度为400像素 + frame.pack_propagate(False) # 阻止子控件改变框架大小 + # 创建标题 + title_label = ttk.Label(frame, text=f"关节控制 - {self.hand_joint}", style='Title.TLabel') + title_label.pack(pady=(0, 10)) + + # 创建滚动框架 + self.canvas = tk.Canvas(frame, bg='#f0f0f0', highlightthickness=0) + self.scrollbar = ttk.Scrollbar(frame, orient=tk.VERTICAL, command=self.canvas.yview) + self.scrollable_frame = ttk.Frame(self.canvas) + + self.scrollable_frame.bind( + "", + self.on_scrollable_frame_configure # 修改为调用方法 + ) + + self.canvas.create_window((0, 0), window=self.scrollable_frame, anchor="nw") + self.canvas.configure(yscrollcommand=self.scrollbar.set) + + # 创建滑动条 + self.create_joint_sliders(self.scrollable_frame) + + self.canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True) + # 初始时不显示滚动条 + self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y) + self.scrollbar.pack_forget() # 隐藏滚动条 + + return frame + def on_scrollable_frame_configure(self, event): + """滚动区域配置变化事件 - 动态显示/隐藏滚动条""" + # 更新滚动区域 + self.canvas.configure(scrollregion=self.canvas.bbox("all")) + + # 检查是否需要显示滚动条 + scrollable_height = self.scrollable_frame.winfo_reqheight() # 内容所需高度 + canvas_height = self.canvas.winfo_height() # 画布实际高度 + + # 如果内容高度大于画布高度，显示滚动条；否则隐藏 + if scrollable_height > canvas_height and canvas_height > 0: + self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y) + else: + self.scrollbar.pack_forget() + + def create_joint_sliders(self, parent): + """创建关节滑动条""" + self.sliders = [] + self.slider_labels = [] + self.slider_frames = [] + + # 存储父框架的引用，用于后续调整大小 + self.sliders_parent = parent + + for i, (name, value) in enumerate(zip( + self.hand_config.joint_names, self.hand_config.init_pos + )): + # 创建框架 - 添加左右边距 + slider_frame = ttk.Frame(parent) + slider_frame.pack(fill=tk.X, pady=5, padx=(10,0)) # 左右各10像素边距 + self.slider_frames.append(slider_frame) + + # 创建标签 + label = ttk.Label(slider_frame, text=f"{name}: {value}", width=15) + label.pack(side=tk.LEFT, padx=(0, 10)) + + # 创建滑动条 - 初始长度设为300，但会随窗口调整 + slider = ttk.Scale(slider_frame, from_=0, to=255, value=value, + orient=tk.HORIZONTAL, length=300, # 初始长度 + command=lambda val, idx=i: self.on_slider_value_changed(idx, val)) + slider.pack(side=tk.LEFT, fill=tk.X, expand=True) + + self.sliders.append(slider) + self.slider_labels.append(label) + + def on_joint_panel_resize(self, event): + """关节控制面板大小变化事件""" + # 获取面板当前宽度 + panel_width = event.width + + # 为所有滑动条设置新的长度 + for slider in self.sliders: + # 计算新的滑动条长度（面板宽度减去标签和其他元素的估计宽度） + # 标签宽度约120像素 + 左右边距各10像素 + 标签与滑动条间距10像素 + new_length = max(panel_width - 150, 100) # 最小长度100像素 + # 重新配置滑动条 + slider.configure(length=new_length-30) + # 面板大小变化后重新检查是否需要滚动条 + self.root.after(100, self.check_scrollbar_visibility) # 延迟检查，确保高度已更新 + + def check_scrollbar_visibility(self): + """检查滚动条可见性""" + # 只有在画布已经有实际高度时才检查 + if self.canvas.winfo_height() > 0: + scrollable_height = self.scrollable_frame.winfo_reqheight() + canvas_height = self.canvas.winfo_height() + + if scrollable_height > canvas_height: + self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y) + else: + self.scrollbar.pack_forget() + + def create_preset_actions_panel(self): + """创建预设动作面板""" + frame = ttk.Frame(self.root) + + # 系统预设动作 + sys_preset_group = ttk.LabelFrame(frame, text="系统预设", style='Group.TLabelframe') + sys_preset_group.pack(fill=tk.BOTH, expand=True, pady=(0, 10)) + + # 创建系统预设动作按钮 - 3列布局 + self.create_system_preset_buttons(sys_preset_group) + + # 动作按钮框架 + actions_frame = ttk.Frame(frame) + actions_frame.pack(fill=tk.X, pady=10) + + # 循环运行按钮 + self.cycle_button = ttk.Button(actions_frame, text="循环预设动作", + command=self.on_cycle_clicked, width=12) + self.cycle_button.pack(side=tk.LEFT, padx=5) + + # 回到初始位置按钮 + self.home_button = ttk.Button(actions_frame, text="回到初始位置", + command=self.on_home_clicked, width=12) + self.home_button.pack(side=tk.LEFT, padx=5) + + # 停止所有动作按钮 + self.stop_button = ttk.Button(actions_frame, text="停止所有动作", + command=self.on_stop_clicked, width=12) + self.stop_button.pack(side=tk.LEFT, padx=5) + + return frame + + def create_system_preset_buttons(self, parent): + """创建系统预设动作按钮 - 3列布局""" + self.preset_buttons = [] + if self.hand_config.preset_actions: + buttons_frame = ttk.Frame(parent) + buttons_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10) + + buttons = [] + for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()): + # 使用 tk.Button 而不是 ttk.Button，以便更好地控制背景色 + button = tk.Button(buttons_frame, text=name, bg='#E6F7FF', fg='#1890FF', width=15, + relief='raised', bd=1, font=('Microsoft YaHei', 10), + command=lambda pos=positions, btn_idx=idx: self.on_preset_action_clicked(pos, btn_idx)) + buttons.append(button) + self.preset_buttons.append(button) + + # 3列布局 + cols = 3 + for i, button in enumerate(buttons): + row, col = divmod(i, cols) + button.grid(row=row, column=col, sticky='ew', padx=5, pady=5) + + # 配置列权重 + for i in range(cols): + buttons_frame.columnconfigure(i, weight=1) + + def create_status_monitor_panel(self): + """创建状态监控面板""" + frame = ttk.Frame(self.root) + + # 标题 + title_label = ttk.Label(frame, text="状态监控", style='Title.TLabel') + title_label.pack(pady=(0, 10)) + + # 快速设置框架 + quick_set_group = ttk.LabelFrame(frame, text="快速设置", style='Group.TLabelframe') + quick_set_group.pack(fill=tk.X, pady=(0, 10)) + + # 速度设置 + speed_frame = ttk.Frame(quick_set_group) + speed_frame.pack(fill=tk.X, padx=10, pady=5) + + ttk.Label(speed_frame, text="速度:").pack(side=tk.LEFT) + self.speed_var = tk.IntVar(value=255) + self.speed_slider = ttk.Scale(speed_frame, from_=0, to=255, + variable=self.speed_var, orient=tk.HORIZONTAL) + self.speed_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5) + self.speed_val_label = ttk.Label(speed_frame, text="255", width=4) + self.speed_val_label.pack(side=tk.LEFT) + self.speed_btn = ttk.Button(speed_frame, text="设置速度", + command=self.on_speed_set) + self.speed_btn.pack(side=tk.LEFT, padx=5) + + # 扭矩设置 + torque_frame = ttk.Frame(quick_set_group) + torque_frame.pack(fill=tk.X, padx=10, pady=5) + + ttk.Label(torque_frame, text="扭矩:").pack(side=tk.LEFT) + self.torque_var = tk.IntVar(value=255) + self.torque_slider = ttk.Scale(torque_frame, from_=0, to=255, + variable=self.torque_var, orient=tk.HORIZONTAL) + self.torque_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5) + self.torque_val_label = ttk.Label(torque_frame, text="255", width=4) + self.torque_val_label.pack(side=tk.LEFT) + self.torque_btn = ttk.Button(torque_frame, text="设置扭矩", + command=self.on_torque_set) + self.torque_btn.pack(side=tk.LEFT, padx=5) + + # 绑定滑块值变化事件 + self.speed_var.trace('w', self.on_speed_changed) + self.torque_var.trace('w', self.on_torque_changed) + + # 创建标签页 + notebook = ttk.Notebook(frame) + notebook.pack(fill=tk.BOTH, expand=True) + + # 系统信息标签页 + sys_info_frame = ttk.Frame(notebook) + notebook.add(sys_info_frame, text="系统信息") + + # 连接状态 - 灰色背景，左对齐 + conn_group = ttk.LabelFrame(sys_info_frame, text="连接状态", style='Group.TLabelframe') + conn_group.pack(fill=tk.X, pady=5) + + # 创建灰色背景的框架 + conn_content_frame = ttk.Frame(conn_group, style='Info.TFrame') + conn_content_frame.pack(fill=tk.X, padx=10, pady=10) + + if self.ros_manager.publisher.get_subscription_count() > 0: + self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点已连接", + foreground="green", background='#e0e0e0', + anchor='w') # 左对齐 + else: + self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点未连接", + foreground="red", background='#e0e0e0', + anchor='w') # 左对齐 + self.connection_status.pack(fill=tk.X) + + # 手部信息 - 灰色背景，左对齐 + hand_info_group = ttk.LabelFrame(sys_info_frame, text="手部信息", style='Group.TLabelframe') + hand_info_group.pack(fill=tk.X, pady=5) + + # 创建灰色背景的框架 + hand_info_content_frame = ttk.Frame(hand_info_group, style='Info.TFrame') + hand_info_content_frame.pack(fill=tk.X, padx=10, pady=10) + + info_text = f"""手部类型: {self.hand_type} +关节型号: {self.hand_joint} +关节数量: {len(self.hand_config.joint_names)} +发布频率: {self.ros_manager.hz} Hz""" + self.hand_info_label = ttk.Label(hand_info_content_frame, text=info_text, + background='#e0e0e0', anchor='w', justify='left') # 左对齐 + self.hand_info_label.pack(fill=tk.X) + + # 状态日志标签页 + log_frame = ttk.Frame(notebook) + notebook.add(log_frame, text="状态日志") + + # 日志文本框 + self.status_log = scrolledtext.ScrolledText(log_frame, height=15, width=50) + self.status_log.pack(fill=tk.BOTH, expand=True, padx=10, pady=10) + self.status_log.insert(tk.END, "等待系统启动...\n") + self.status_log.config(state=tk.DISABLED) + + # 清除日志按钮 + clear_log_btn = ttk.Button(log_frame, text="清除日志", + command=self.clear_status_log) + clear_log_btn.pack(pady=5) + + return frame + + def create_value_display_panel(self): + """创建滑动条数值显示面板""" + frame = ttk.LabelFrame(self.root, text="关节数值列表", style='Group.TLabelframe') + + # 创建按钮框架（放在显示框上方） + button_frame = ttk.Frame(frame) + button_frame.pack(fill=tk.X, padx=10, pady=(10, 5)) + + # 复制按钮 - 左对齐 + copy_button = ttk.Button(button_frame, text="复制到剪切板", + command=self.copy_values_to_clipboard, width=10) + copy_button.pack(side=tk.LEFT) + + # 数值显示框 + self.value_display = scrolledtext.ScrolledText(frame, height=4, width=100) + self.value_display.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10)) + self.value_display.insert(tk.END, "[]") + self.value_display.config(state=tk.DISABLED) + + return frame + + def copy_values_to_clipboard(self): + """复制关节数值到系统剪切板""" + try: + # 获取当前按钮引用 + button = self.root.focus_get() + original_text = "复制到剪切板" + + # 获取文本框内容 + content = self.value_display.get(1.0, tk.END).strip() + + # 清除文本框的选中状态 + self.value_display.tag_remove(tk.SEL, "1.0", tk.END) + + # 复制到剪切板 + self.root.clipboard_clear() + self.root.clipboard_append(content) + + # 改变按钮文本提示复制成功 + if isinstance(button, ttk.Button): + button.config(text="已复制!") + # 1.5秒后恢复原文本 + self.root.after(1500, lambda: button.config(text=original_text)) + + self.update_status("info", f"关节数值已复制到剪切板") + + except Exception as e: + self.update_status("error", f"复制失败: {str(e)}") + + def on_slider_value_changed(self, index: int, value: str): + """滑动条值改变事件处理""" + value_int = int(float(value)) + if 0 <= index < len(self.slider_labels): + joint_name = self.hand_config.joint_names[index] + self.slider_labels[index].config(text=f"{joint_name}: {value_int}") + + # 更新数值显示 + self.update_value_display() + + def update_value_display(self): + """更新数值显示面板内容""" + values = [int(float(slider.get())) for slider in self.sliders] + + self.value_display.config(state=tk.NORMAL) + self.value_display.delete(1.0, tk.END) + self.value_display.insert(tk.END, f"{values}") + self.value_display.config(state=tk.DISABLED) + + def on_preset_action_clicked(self, positions: List[int], button_index: int = None): + """预设动作按钮点击事件处理""" + if len(positions) != len(self.sliders): + messagebox.showwarning( + "动作不匹配", + f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配" + ) + return + + # 重置所有按钮颜色 + self.reset_preset_buttons_color() + + # 高亮当前点击的按钮 + if button_index is not None and 0 <= button_index < len(self.preset_buttons): + self.preset_buttons[button_index].config(bg='#1890FF', fg='white') + self.current_clicked_button = button_index + + # 更新滑动条 + for i, (slider, pos) in enumerate(zip(self.sliders, positions)): + slider.set(pos) + self.on_slider_value_changed(i, str(pos)) + + # 发布关节状态 + self.publish_joint_state() + + def on_home_clicked(self): + """回到初始位置按钮点击事件处理""" + for slider, pos in zip(self.sliders, self.hand_config.init_pos): + slider.set(pos) + + self.publish_joint_state() + self.update_status("info", "回到初始位置") + + # 更新数值显示 + self.update_value_display() + + def on_stop_clicked(self): + """停止所有动作按钮点击事件处理""" + # 停止循环定时器 + if self.cycle_timer: + self.root.after_cancel(self.cycle_timer) + self.cycle_timer = None + self.cycle_button.config(text="循环运行预设动作") + self.reset_preset_buttons_color() + + self.update_status("warning", "已停止所有动作") + + def on_cycle_clicked(self): + """循环运行预设动作按钮点击事件处理""" + if not self.hand_config.preset_actions: + messagebox.showwarning("无预设动作", "当前手部型号没有预设动作可循环运行") + return + + if self.cycle_timer: + # 停止循环 + self.root.after_cancel(self.cycle_timer) + self.cycle_timer = None + self.cycle_button.config(text="循环运行预设动作") + self.reset_preset_buttons_color() + # 如果有之前点击的按钮，恢复其点击状态 + if hasattr(self, 'current_clicked_button') and self.current_clicked_button is not None: + if 0 <= self.current_clicked_button < len(self.preset_buttons): + self.preset_buttons[self.current_clicked_button].config(bg='#1890FF', fg='white') + self.update_status("info", "已停止循环运行预设动作") + else: + # 开始循环 + self.current_action_index = -1 + self.cycle_button.config(text="停止循环运行") + self.update_status("info", "开始循环运行预设动作") + self.run_next_action() + + def run_next_action(self): + """运行下一个预设动作""" + if not self.hand_config.preset_actions: + return + + # 重置所有按钮颜色 + self.reset_preset_buttons_color() + + # 计算下一个动作索引 + self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions) + + # 获取下一个动作 + action_names = list(self.hand_config.preset_actions.keys()) + action_name = action_names[self.current_action_index] + action_positions = self.hand_config.preset_actions[action_name] + + # 执行动作 + self.on_preset_action_clicked(action_positions, self.current_action_index) + + # 高亮当前动作按钮（循环模式使用不同的颜色） + if 0 <= self.current_action_index < len(self.preset_buttons): + button = self.preset_buttons[self.current_action_index] + button.config(bg='#52C41A', fg='white') # 绿色表示循环中的按钮 + + self.update_status("info", f"运行预设动作: {action_name}") + + # 设置下一个动作定时器 + self.cycle_timer = self.root.after(LOOP_TIME, self.run_next_action) + + def reset_preset_buttons_color(self): + """重置所有预设按钮颜色""" + for button in self.preset_buttons: + button.config(bg='#E6F7FF', fg='#1890FF') # 恢复默认颜色 + + def on_speed_changed(self, *args): + """速度滑块值改变事件""" + self.speed_val_label.config(text=str(self.speed_var.get())) + + def on_torque_changed(self, *args): + """扭矩滑块值改变事件""" + self.torque_val_label.config(text=str(self.torque_var.get())) + + def on_speed_set(self): + """设置速度""" + speed_val = self.speed_var.get() + self.ros_manager.publish_speed(speed_val) + self.update_status("info", f"速度已设为 {speed_val}") + + def on_torque_set(self): + """设置扭矩""" + torque_val = self.torque_var.get() + self.ros_manager.publish_torque(torque_val) + self.update_status("info", f"扭矩已设为 {torque_val}") + + def publish_joint_state(self): + """发布当前关节状态""" + positions = [int(float(slider.get())) for slider in self.sliders] + self.ros_manager.publish_joint_state(positions) + + def start_publish_timer(self): + """开始发布定时器""" + self.publish_joint_state() + interval = int(1000 / self.ros_manager.hz) + self.publish_timer_id = self.root.after(interval, self.start_publish_timer) + + def update_status(self, status_type: str, message: str): + """更新状态显示""" + # 更新连接状态 + if status_type == "info" and "ROS2节点初始化成功" in message: + self.connection_status.config(text="ROS2节点已连接", foreground="green") + + # 更新日志 + current_time = time.strftime("%H:%M:%S") + log_entry = f"[{current_time}] {message}\n" + + self.status_log.config(state=tk.NORMAL) + self.status_log.insert(tk.END, log_entry) + self.status_log.see(tk.END) + + # 限制日志长度 + log_content = self.status_log.get(1.0, tk.END) + if len(log_content) > 10000: + self.status_log.delete(1.0, f"{len(log_content)-10000}.0") + + self.status_log.config(state=tk.DISABLED) + + # 设置日志颜色 + if status_type == "error": + # 可以在Tkinter中为不同消息类型添加颜色标记 + pass + + def clear_status_log(self): + """清除状态日志""" + self.status_log.config(state=tk.NORMAL) + self.status_log.delete(1.0, tk.END) + self.status_log.insert(tk.END, "日志已清除\n") + self.status_log.config(state=tk.DISABLED) + + def run(self): + """运行GUI""" + try: + self.root.mainloop() + finally: + # 清理定时器 + if self.cycle_timer: + self.root.after_cancel(self.cycle_timer) + if self.publish_timer_id: + self.root.after_cancel(self.publish_timer_id) + # 关闭ROS2节点 + self.ros_manager.shutdown() + +def main(args=None): + """主函数""" + try: + # 创建ROS2节点管理器 + ros_manager = ROS2NodeManager() + + # 创建GUI + gui = HandControlGUI(ros_manager) + + # 运行应用 + gui.run() + + except Exception as e: + print(f"应用程序启动失败: {str(e)}") + sys.exit(1) + +if __name__ == '__main__': + main() diff --git a/src/gui_control/gui_control/utils/color_msg.py b/src/gui_control/gui_control/utils/color_msg.py new file mode 100644 index 0000000..059d60a --- /dev/null +++ b/src/gui_control/gui_control/utils/color_msg.py @@ -0,0 +1,27 @@ +#! /usr/bin/env python3 + +import time + +class ColorMsg(): + def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None: + self.msg = msg + self.color = color + self.timestamp = timestamp + self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp) + + def colorMsg(self,msg: str, color: str = '', timestamp: bool = True): + str = "" + if timestamp: + str += time.strftime('%Y-%m-%d %H:%M:%S', + time.localtime(time.time())) + " " + if color == "red": + str += "\033[1;31;40m" + elif color == "green": + str += "\033[1;32;40m" + elif color == "yellow": + str += "\033[1;33;40m" + else: + print(str + msg) + return + str += msg + "\033[0m" + print(str) \ No newline at end of file diff --git a/src/gui_control/gui_control/utils/mapping.py b/src/gui_control/gui_control/utils/mapping.py new file mode 100644 index 0000000..deb6a19 --- /dev/null +++ b/src/gui_control/gui_control/utils/mapping.py @@ -0,0 +1,383 @@ +#--------------------------------------------------------------------------------------------------- +# L6 L +l6_l_min = [0, 0, 0, 0, 0, 0] +l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26] +l6_l_derict = [-1, -1, -1, -1, -1, -1] +# L6 R +l6_r_min = [0, 0, 0, 0, 0, 0] +l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26] +l6_r_derict = [-1, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# O6 L +o6_l_min = [0, 0, 0, 0, 0, 0] +o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6] +o6_l_derict = [-1, -1, -1, -1, -1, -1] +# O6 R +o6_r_min = [0, 0, 0, 0, 0, 0] +o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6] +o6_r_derict = [-1, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# L7 L OK +l7_l_min = [0, 0, 0, 0, 0, 0, 0] +l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01] +l7_l_derict = [-1, -1, -1, -1, -1, -1, -1] +# L7 R OK (urdf后续会更改！！！) +l7_r_min = [0, -1.43, 0, 0, 0, 0, 0] +l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54] +l7_r_derict = [-1, 0, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# L10 L OK +l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52] +l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01] +l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1] +# L10 R OK +l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52] +l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01] +l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1] +#--------------------------------------------------------------------------------------------------- +# L20 L OK +l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08] +l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +# L20 R OK +l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08] +l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +# L21 L OK +l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57] +l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +# L21 R OK +l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57] +l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- +#--------------------------------------------------------------------------------------------------- +# L25 L OK +l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57] +l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1] +# L25 R OK +l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57] +l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1] +#--------------------------------------------------------------------------------------------------- + +def range_to_arc_left(left_range,hand_joint): + num=0 + if hand_joint == "L6": + num = 6 + l_min = l6_l_min + l_max = l6_l_max + l_derict = l6_l_derict + elif hand_joint == "O6": + num = 6 + l_min = o6_l_min + l_max = o6_l_max + l_derict = o6_l_derict + elif hand_joint == "L7": + num = 7 + l_min = l7_l_min + l_max = l7_l_max + l_derict = l7_l_derict + elif hand_joint == "L10": + num = 10 + l_min = l10_l_min + l_max = l10_l_max + l_derict = l10_l_derict + elif hand_joint == "L20": + num = 20 + l_min = l20_l_min + l_max = l20_l_max + l_derict = l20_l_derict + elif hand_joint == "L21": + num = 25 + l_min = l21_l_min + l_max = l21_l_max + l_derict = l21_l_derict + hand_arc = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_l = is_within_range(left_range[i], 0, 255) + if l_derict[i] == -1: + hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i]) + else: + hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i]) + return hand_arc + +def range_to_arc_right(right_range,hand_joint): + num=0 + if hand_joint == "L6": + num = 6 + r_min = l6_r_min + r_max = l6_r_max + r_derict = l6_r_derict + elif hand_joint == "O6": + num = 6 + r_min = o6_r_min + r_max = o6_r_max + r_derict = o6_r_derict + elif hand_joint == "L7": + num = 7 + r_min = l7_r_min + r_max = l7_r_max + r_derict = l7_r_derict + elif hand_joint == "L10": + num = 10 + r_min = l10_r_min + r_max = l10_r_max + r_derict = l10_r_derict + elif hand_joint == "L20": + num = 20 + r_min = l20_r_min + r_max = l20_r_max + r_derict = l20_r_derict + elif hand_joint == "L21": + num = 25 + r_min = l21_r_min + r_max = l21_r_max + r_derict = l21_r_derict + hand_arc = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_r = is_within_range(right_range[i], 0, 255) + if r_derict[i] == -1: + hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i]) + else: + hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i]) + return hand_arc + +''' +def arc_to_range_left(left_arc,hand_joint): + num=0 + if hand_joint == "L7": + num = 7 + l_min = l7_l_min + l_max = l7_l_max + l_derict = l7_l_derict + elif hand_joint == "L10": + num = 10 + l_min = l10_l_min + l_max = l10_l_max + l_derict = l10_l_derict + elif hand_joint == "L20": + num = 20 + l_min = l20_l_min + l_max = l20_l_max + l_derict = l20_l_derict + elif hand_joint == "L21": + num = 25 + l_min = l21_l_min + l_max = l21_l_max + l_derict = l21_l_derict + hand_range = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_l = is_within_range(left_arc[i], 0, 255) + if l_derict[i] == -1: + hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i]) + else: + hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i]) + return hand_range + ''' +def arc_to_range_left(hand_arc_l,hand_joint): + num=0 + if hand_joint == "O6": + num = 6 + l_min = o6_l_min + l_max = o6_l_max + l_derict = o6_l_derict + elif hand_joint == "L7": + num = 7 + l_min = l7_l_min + l_max = l7_l_max + l_derict = l7_l_derict + elif hand_joint == "L10": + num = 10 + l_min = l10_l_min + l_max = l10_l_max + l_derict = l10_l_derict + elif hand_joint == "L20": + num = 20 + l_min = l20_l_min + l_max = l20_l_max + l_derict = l20_l_derict + elif hand_joint == "L21": + num = 25 + l_min = l21_l_min + l_max = l21_l_max + l_derict = l21_l_derict + hand_range = [0] * num + #hand_range_l = [0] * 7 + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i]) + if l_derict[i] == -1: + hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0) + else: + hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255) + + return hand_range + +def arc_to_range_right(right_arc,hand_joint): + num=0 + if hand_joint == "O6": + num = 6 + r_min = o6_r_min + r_max = o6_r_max + r_derict = o6_r_derict + elif hand_joint == "L7": + num = 7 + r_min = l7_r_min + r_max = l7_r_max + r_derict = l7_r_derict + elif hand_joint == "L10": + num = 10 + r_min = l10_r_min + r_max = l10_r_max + r_derict = l10_r_derict + elif hand_joint == "L20": + num = 20 + r_min = l20_r_min + r_max = l20_r_max + r_derict = l20_r_derict + elif hand_joint == "L21": + num = 25 + r_min = l21_r_min + r_max = l21_r_max + r_derict = l21_r_derict + hand_range = [0] * num + for i in range(num): + if hand_joint == "L20": + if 11 <= i <= 14: continue + if hand_joint == "L21": + if 11 <= i <= 14: continue + if 16 <= i <= 19: continue + val_r = is_within_range(right_arc[i], r_min[i], r_max[i]) + if r_derict[i] == -1: + hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0) + else: + hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255) + return hand_range + + + + +def range_to_arc_right_l20(hand_range_r): + hand_arc_r = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_r = is_within_range(hand_range_r[i], 0, 255) + if l20_r_derict[i] == -1: + hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i]) + else: + hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i]) + return hand_arc_r + + +def range_to_arc_left_l20(hand_range_l): + hand_arc_l = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_l = is_within_range(hand_range_l[i], 0, 255) + if l20_l_derict[i] == -1: + hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i]) + else: + hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i]) + return hand_arc_l + + +def arc_to_range_right_l20(hand_arc_r): + hand_range_r = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i]) + if l20_r_derict[i] == -1: + hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0) + else: + hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255) + return hand_range_r + + +def arc_to_range_left_l20(hand_arc_l): + hand_range_l = [0] * 20 + for i in range(20): + if 11 <= i <= 14: continue + val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i]) + if l20_l_derict[i] == -1: + hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0) + else: + hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255) + + return hand_range_l + + +def range_to_arc_right_10(hand_range_r): + hand_arc_r = [0] * 10 + for i in range(10): + val_r = is_within_range(hand_range_r[i], 0, 255) + if l10_r_derict[i] == -1: + hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i]) + else: + hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i]) + + return hand_arc_r + + +def range_to_arc_left_10(hand_range_l): + hand_arc_l = [0] * 10 + for i in range(10): + val_l = is_within_range(hand_range_l[i], 0, 255) + if l10_l_derict[i] == -1: + hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i]) + else: + hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i]) + return hand_arc_l + + +def arc_to_range_right_10(hand_arc_r): + hand_range_r = [0] * 10 + for i in range(10): + val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i]) + if l10_r_derict[i] == -1: + hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0) + else: + hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255) + return hand_range_r + + +def arc_to_range_left_10(hand_arc_l): + hand_range_l = [0] * 10 + for i in range(10): + val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i]) + if l10_l_derict[i] == -1: + hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0) + else: + hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255) + + return hand_range_l + + +def scale_value(original_value, a_min, a_max, b_min, b_max): + return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min + + +def is_within_range(value, min_value, max_value): + return min(max_value, max(min_value, value)) diff --git a/src/gui_control/launch/gui_control.launch.py b/src/gui_control/launch/gui_control.launch.py new file mode 100644 index 0000000..49c3996 --- /dev/null +++ b/src/gui_control/launch/gui_control.launch.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def generate_launch_description(): + # 声明参数：是否显示压力图 + declare_show_diagram = DeclareLaunchArgument( + 'show_pressure_diagram', + default_value='true', + description='是否启动压力图窗口: true | false' + ) + + # 根据 show_pressure_diagram 参数决定是否包含 pressure_diagram launch 文件 + pressure_diagram_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('pressure_diagram'), + 'launch', + 'pressure_diagram.launch.py' + ]) + ), + condition=IfCondition(LaunchConfiguration('show_pressure_diagram')) + ) + + return LaunchDescription([ + declare_show_diagram, + pressure_diagram_launch, + Node( + package='gui_control', + executable='gui_control', + name='right_hand_control_node', + output='screen', + parameters=[{ + 'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写 + 'hand_joint': "L10", # O6\L6\L7\L10\L20\G20(工业版)\L21 字母为大写 + 'topic_hz': 30, # topic发布频率 + 'is_touch': False, # 是否有压力传感器 + 'is_arc': False, # 是否发布弧度值topic + }], + ), + # Node( + # package='gui_control', + # executable='gui_control', + # name='right_hand_control_node', + # output='screen', + # parameters=[{ + # 'hand_type': 'right', + # 'hand_joint': "L10", + # 'topic_hz': 30, + # 'is_touch': True, + # }], + # ), + ]) diff --git a/src/gui_control/package.xml b/src/gui_control/package.xml new file mode 100644 index 0000000..b444511 --- /dev/null +++ b/src/gui_control/package.xml @@ -0,0 +1,21 @@ + + + + gui_control + 0.0.0 + TODO: Package description + linker-robot + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + linker_hand_ros2_sdk + + + ament_python + + + diff --git a/src/gui_control/pyproject.toml b/src/gui_control/pyproject.toml new file mode 100644 index 0000000..638dd9c --- /dev/null +++ b/src/gui_control/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" diff --git a/src/gui_control/resource/gui_control b/src/gui_control/resource/gui_control new file mode 100644 index 0000000..e69de29 diff --git a/src/gui_control/setup.cfg b/src/gui_control/setup.cfg new file mode 100644 index 0000000..4691687 --- /dev/null +++ b/src/gui_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/gui_control +[install] +install_scripts=$base/lib/gui_control diff --git a/src/gui_control/setup.py b/src/gui_control/setup.py new file mode 100644 index 0000000..9f7a812 --- /dev/null +++ b/src/gui_control/setup.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import os +from glob import glob +from setuptools import find_packages, setup +package_name = 'gui_control' +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + (os.path.join('share', 'gui_control', 'launch'), glob('launch/*.launch.py')), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='linker-robot', + maintainer_email='linker-robot@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': ['pytest'], + }, + entry_points={ + 'console_scripts': [ + 'gui_control = gui_control.gui_control:main' + ], + }, +) diff --git a/src/gui_control/test/test_copyright.py b/src/gui_control/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/gui_control/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/gui_control/test/test_flake8.py b/src/gui_control/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/gui_control/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/gui_control/test/test_pep257.py b/src/gui_control/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/gui_control/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml index 76f6860..f3a7511 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml @@ -5,7 +5,7 @@ LINKER_HAND: # 手部配置信息 TOUCH: True # 是否有压力传感器 CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0 MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中 - JOINT: L6 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/ + JOINT: L10 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/ NAME: # 默认值，不用修改 - joint41 - joint42 @@ -33,7 +33,7 @@ LINKER_HAND: # 手部配置信息 TOUCH: False # 是否有压力传感器 CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0 MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中 - JOINT: L6 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/ + JOINT: L10 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/ NAME: # 默认值，不用修改 - joint71 - joint72 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py index 45d729e..f8716a5 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py @@ -398,11 +398,12 @@ def main(args=None): try: rclpy.init(args=args) node = LinkerHand("linker_hand_sdk") - embedded_version = node.embedded_version - if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20": + embedded_version = node.embedded_version or [] + hand_joint = node.hand_joint.upper() + if len(embedded_version) == 3 or hand_joint in {"O6", "L6", "G20"}: ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green") node.sdk_v = 2 - elif len(embedded_version) == 6 and node.hand_joint == "L10": + elif len(embedded_version) == 6 and hand_joint == "L10": ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green") node.sdk_v = 2 elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)): diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py new file mode 100644 index 0000000..2a06b5a --- /dev/null +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""Record O6 joint positions and plot angle/derived-velocity curves. + +The O6 ``0x05`` value exposed by the SDK is a configured speed, not a measured +joint velocity. This recorder therefore always derives velocity from the +position samples and their monotonic timestamps. +""" + +import argparse +import csv +from datetime import datetime +import json +import math +from pathlib import Path +import sys +import time + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.utilities import remove_ros_args +from sensor_msgs.msg import JointState + + +O6_JOINT_NAMES = ( + "thumb_cmc_pitch", + "thumb_cmc_yaw", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", +) + +# Nominal SDK ranges. A hand-specific lookup JSON should be used for formal +# measurements because individual hands can differ from these values. +O6_NOMINAL_MAX_DEG = dict( + zip(O6_JOINT_NAMES, np.rad2deg([0.58, 1.36, 1.6, 1.6, 1.6, 1.6])) +) + + +class AngleMapper: + """Convert O6 uint8 positions to degrees using lookup tables or nominal ranges.""" + + def __init__(self, mapping_json=None): + self.tables = {} + self.source = "SDK nominal range" + if mapping_json: + self._load(Path(mapping_json).expanduser()) + + def _load(self, path): + with path.open("r", encoding="utf-8") as stream: + data = json.load(stream) + + joints = data.get("joints", data) + if not isinstance(joints, dict): + raise ValueError("mapping JSON must contain an object named 'joints'") + + for joint_name, values in joints.items(): + if not isinstance(values, dict): + continue + joint_u8 = values.get("joint_u8") + joint_real_deg = values.get("joint_real_deg") + if joint_u8 is None or joint_real_deg is None: + continue + if len(joint_u8) != len(joint_real_deg) or len(joint_u8) < 2: + raise ValueError( + f"{joint_name}: joint_u8 and joint_real_deg need equal lengths >= 2" + ) + + x = np.asarray(joint_u8, dtype=float) + y = np.asarray(joint_real_deg, dtype=float) + if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)): + raise ValueError(f"{joint_name}: mapping contains a non-finite value") + order = np.argsort(x) + x = x[order] + y = y[order] + if np.any(np.diff(x) <= 0): + raise ValueError(f"{joint_name}: joint_u8 values must be unique") + self.tables[joint_name] = (x, y) + + if not self.tables: + raise ValueError( + "mapping JSON has no joint with joint_u8 and joint_real_deg arrays" + ) + self.source = str(path) + + def to_degrees(self, joint_name, position_u8): + if joint_name in self.tables: + x, y = self.tables[joint_name] + return float(np.interp(position_u8, x, y)) + + if joint_name not in O6_NOMINAL_MAX_DEG: + raise KeyError(f"no O6 nominal range or lookup table for {joint_name}") + position_u8 = float(np.clip(position_u8, 0.0, 255.0)) + return (255.0 - position_u8) * O6_NOMINAL_MAX_DEG[joint_name] / 255.0 + + +def derive_velocity(time_s, angle_deg, cutoff_hz=5.0): + """Differentiate angle samples and apply a first-order low-pass filter.""" + time_s = np.asarray(time_s, dtype=float) + angle_deg = np.asarray(angle_deg, dtype=float) + if len(time_s) != len(angle_deg): + raise ValueError("time and angle arrays must have the same length") + if len(time_s) < 2: + return np.zeros_like(angle_deg) + if np.any(np.diff(time_s) <= 0): + raise ValueError("timestamps must be strictly increasing") + + velocity = np.gradient(angle_deg, time_s) + if cutoff_hz <= 0.0: + return velocity + + filtered = np.empty_like(velocity) + filtered[0] = velocity[0] + rc = 1.0 / (2.0 * math.pi * cutoff_hz) + for index in range(1, len(velocity)): + dt = time_s[index] - time_s[index - 1] + alpha = dt / (rc + dt) + filtered[index] = filtered[index - 1] + alpha * ( + velocity[index] - filtered[index - 1] + ) + return filtered + + +class O6JointCurveRecorder(Node): + def __init__( + self, + hand_type, + duration, + output_dir, + joint_name, + mapping_json, + filter_hz, + ): + super().__init__("o6_joint_curve") + self.hand_type = hand_type + self.duration = duration + self.output_dir = Path(output_dir).expanduser() + self.joint_name = joint_name + self.filter_hz = filter_hz + self.mapper = AngleMapper(mapping_json) + self.samples = [] + self.start_time = None + self.saved_paths = None + self.finished = False + + topic = f"/cb_{hand_type}_hand_state" + self.subscription = self.create_subscription( + JointState, topic, self._state_callback, 100 + ) + self.stop_timer = self.create_timer(0.05, self._check_duration) + self.get_logger().info( + f"Recording {topic}; duration={duration:.3f}s; angle map={self.mapper.source}" + ) + + def _state_callback(self, msg): + if self.finished or not msg.position: + return + + names = tuple(msg.name) if len(msg.name) == len(msg.position) else O6_JOINT_NAMES + if len(msg.position) != len(names): + self.get_logger().warning( + f"Ignoring JointState with {len(msg.position)} positions and " + f"{len(msg.name)} names" + ) + return + + now = time.perf_counter() + if self.start_time is None: + self.start_time = now + relative_time = now - self.start_time + for name, position in zip(names, msg.position): + if self.joint_name != "all" and name != self.joint_name: + continue + try: + angle_deg = self.mapper.to_degrees(name, position) + except KeyError as error: + self.get_logger().warning(str(error)) + continue + self.samples.append((relative_time, name, float(position), angle_deg)) + + def _check_duration(self): + if self.finished or self.start_time is None or self.duration <= 0.0: + return + if time.perf_counter() - self.start_time >= self.duration: + self.finish() + rclpy.shutdown() + + def finish(self): + if self.finished: + return self.saved_paths + self.finished = True + if not self.samples: + self.get_logger().warning("No joint-state sample received; no files were written") + return None + self.saved_paths = export_curves( + samples=self.samples, + output_dir=self.output_dir, + hand_type=self.hand_type, + mapping_source=self.mapper.source, + cutoff_hz=self.filter_hz, + ) + csv_path, plot_path = self.saved_paths + self.get_logger().info(f"Saved samples: {csv_path}") + self.get_logger().info(f"Saved curves: {plot_path}") + return self.saved_paths + + +def export_curves(samples, output_dir, hand_type, mapping_source, cutoff_hz): + """Export long-form CSV and a two-panel PNG for all recorded joints.""" + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + output_dir = Path(output_dir).expanduser() + output_dir.mkdir(parents=True, exist_ok=True) + stem = f"o6_{hand_type}_{datetime.now().strftime('%Y%m%d_%H%M%S')}" + csv_path = output_dir / f"{stem}.csv" + plot_path = output_dir / f"{stem}.png" + + joint_samples = {} + for sample in samples: + joint_samples.setdefault(sample[1], []).append(sample) + + processed = {} + for joint_name, values in joint_samples.items(): + time_s = np.asarray([value[0] for value in values], dtype=float) + position_u8 = np.asarray([value[2] for value in values], dtype=float) + angle_deg = np.asarray([value[3] for value in values], dtype=float) + velocity_deg_s = derive_velocity(time_s, angle_deg, cutoff_hz) + processed[joint_name] = (time_s, position_u8, angle_deg, velocity_deg_s) + + with csv_path.open("w", encoding="utf-8", newline="") as stream: + writer = csv.writer(stream) + writer.writerow( + ["t_s", "joint_name", "position_u8", "angle_deg", "velocity_deg_s"] + ) + for joint_name, arrays in processed.items(): + for row in zip(*arrays): + writer.writerow( + [ + f"{row[0]:.9f}", + joint_name, + f"{row[1]:.6f}", + f"{row[2]:.6f}", + f"{row[3]:.6f}", + ] + ) + + figure, axes = plt.subplots(2, 1, sharex=True, figsize=(12, 8)) + for joint_name, (time_s, _position_u8, angle_deg, velocity_deg_s) in processed.items(): + axes[0].plot(time_s, angle_deg, label=joint_name) + axes[1].plot(time_s, velocity_deg_s, label=joint_name) + + axes[0].set_ylabel("Angle (deg)") + axes[0].set_title(f"O6 {hand_type} joint curves\nangle map: {mapping_source}") + axes[1].set_ylabel("Velocity (deg/s)") + axes[1].set_xlabel("Time (s)") + axes[1].set_title(f"Derived velocity, first-order low-pass {cutoff_hz:g} Hz") + for axis in axes: + axis.grid(True, alpha=0.3) + axis.legend(loc="best", fontsize="small") + figure.tight_layout() + figure.savefig(plot_path, dpi=160) + plt.close(figure) + return csv_path, plot_path + + +def _parse_args(argv): + parser = argparse.ArgumentParser( + description="Record O6 angle and angle-derived velocity curves" + ) + parser.add_argument("--hand-type", choices=("left", "right"), default="right") + parser.add_argument( + "--duration", + type=float, + default=10.0, + help="recording seconds; <= 0 records until Ctrl+C (default: 10)", + ) + parser.add_argument( + "--output-dir", default="logs/O6/curves", help="CSV/PNG output directory" + ) + parser.add_argument( + "--joint", + default="all", + choices=("all",) + O6_JOINT_NAMES, + help="plot one joint or all six (default: all)", + ) + parser.add_argument( + "--mapping-json", + help="optional hand_.json containing joints..joint_u8 and joint_real_deg", + ) + parser.add_argument( + "--filter-hz", + type=float, + default=5.0, + help="velocity low-pass cutoff; <= 0 disables filtering (default: 5)", + ) + parsed = parser.parse_args(remove_ros_args(args=argv)[1:]) + if parsed.duration == 0.0: + parsed.duration = -1.0 + return parsed + + +def main(args=None): + argv = sys.argv if args is None else [sys.argv[0], *args] + parsed = _parse_args(argv) + rclpy.init(args=args) + node = O6JointCurveRecorder( + hand_type=parsed.hand_type, + duration=parsed.duration, + output_dir=parsed.output_dir, + joint_name=parsed.joint, + mapping_json=parsed.mapping_json, + filter_hz=parsed.filter_hz, + ) + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Stopping on Ctrl+C") + finally: + node.finish() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/linker_hand_ros2_sdk/package.xml b/src/linker_hand_ros2_sdk/package.xml index 665b626..943c8c2 100644 --- a/src/linker_hand_ros2_sdk/package.xml +++ b/src/linker_hand_ros2_sdk/package.xml @@ -14,6 +14,9 @@ rclpy launch + sensor_msgs + python3-numpy + python3-matplotlib ament_python diff --git a/src/linker_hand_ros2_sdk/setup.py b/src/linker_hand_ros2_sdk/setup.py index 2e4ed2e..5a1479f 100644 --- a/src/linker_hand_ros2_sdk/setup.py +++ b/src/linker_hand_ros2_sdk/setup.py @@ -40,6 +40,7 @@ setup( 'console_scripts': [ 'linker_hand_sdk = linker_hand_ros2_sdk.linker_hand:main', 'linker_hand_advanced_o6 = linker_hand_ros2_sdk.linker_hand_advanced_o6:main', + 'o6_joint_curve = linker_hand_ros2_sdk.o6_joint_curve:main', 'linker_hand_advanced_l6 = linker_hand_ros2_sdk.linker_hand_advanced_l6:main', 'linker_hand_advanced_l7 = linker_hand_ros2_sdk.linker_hand_advanced_l7:main', 'linker_hand_advanced_l10 = linker_hand_ros2_sdk.linker_hand_advanced_l10:main', diff --git a/src/pressure_diagram/launch/pressure_diagram.launch.py b/src/pressure_diagram/launch/pressure_diagram.launch.py new file mode 100644 index 0000000..34a5800 --- /dev/null +++ b/src/pressure_diagram/launch/pressure_diagram.launch.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='pressure_diagram', + executable='pressure_diagram', + name='pressure_diagram_node', + output='screen', + parameters=[{ + + }], + ), + + ]) \ No newline at end of file diff --git a/src/pressure_diagram/package.xml b/src/pressure_diagram/package.xml new file mode 100644 index 0000000..15650cc --- /dev/null +++ b/src/pressure_diagram/package.xml @@ -0,0 +1,28 @@ + + + + pressure_diagram + 0.0.0 + ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand + linkerhand + Apache-2.0 + + + rclpy + std_msgs + ament_index_python + + + python3-pyqt5 + python3-pyqtgraph + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/pressure_diagram/pressure_diagram/__init__.py b/src/pressure_diagram/pressure_diagram/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/pressure_diagram/pressure_diagram/pressure_diagram.py b/src/pressure_diagram/pressure_diagram/pressure_diagram.py new file mode 100644 index 0000000..9e78244 --- /dev/null +++ b/src/pressure_diagram/pressure_diagram/pressure_diagram.py @@ -0,0 +1,421 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +import signal +import sys +import json +import numpy as np +from std_msgs.msg import String +from PyQt5 import QtWidgets, QtCore, QtGui +import pyqtgraph as pg + +pg.setConfigOption('imageAxisOrder', 'row-major') +pg.setConfigOption('useOpenGL', False) + +class PressureDiagram(Node, QtWidgets.QMainWindow): + def __init__(self): + # 先调用 Node 的 __init__，避免 super() 歧义 + Node.__init__(self, 'pressure_diagram') + QtWidgets.QMainWindow.__init__(self) + + # 键名与ROS数据一致 + self.fingers_matrix = ['thumb_matrix', 'index_matrix', 'middle_matrix', 'ring_matrix', 'little_matrix'] + self.fingers_mass = ['thumb_mass', 'index_mass', 'middle_mass', 'ring_mass', 'little_mass'] + + self.finger_colors = [ + (255, 75, 75), # Thumb - Red + (50, 205, 50), # Index - Green + (65, 105, 255), # Middle - Blue + (255, 215, 0), # Ring - Gold + (218, 112, 214) # Pinky - Purple + ] + + # 波形图数据 + self.wave_data = {f: np.zeros(100) for f in self.fingers_mass} + self.current_wave_values = {f: 0.0 for f in self.fingers_mass} + + # 热力图数据 - 12行×6列 (高大于宽) + self.matrix_data = {f: np.zeros((12, 6)) for f in self.fingers_matrix} + self.matrix_shapes = {f: (12, 6) for f in self.fingers_matrix} # (rows, cols) + self.data_received = {f: False for f in self.fingers_matrix} + + self.wave_sub = None + self.matrix_sub = None + + # ROS2 节流计时器 + self.last_matrix_log_time = {f: None for f in self.fingers_matrix} + + # QoS 配置 + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=100 + ) + self.qos_profile = qos_profile + + self.init_ui() + self.setup_ros() + + def init_ui(self): + self.setWindowTitle("Robotic Hand Sensor Fusion Interface") + self.resize(1600, 900) + self.setStyleSheet("background-color: #0f172a;") + + central_widget = QtWidgets.QWidget() + self.setCentralWidget(central_widget) + + main_layout = QtWidgets.QHBoxLayout(central_widget) + main_layout.setSpacing(15) + main_layout.setContentsMargins(10, 10, 10, 10) + + # ============== 左侧：波形图 ============== + left_widget = self.create_waveform_panel() + main_layout.addWidget(left_widget, stretch=1) + + line = QtWidgets.QFrame() + line.setFrameShape(QtWidgets.QFrame.VLine) + line.setStyleSheet("QFrame { background-color: #334155; max-width: 2px; }") + main_layout.addWidget(line) + + # ============== 右侧：热力图 ============== + right_widget = self.create_heatmap_panel() + main_layout.addWidget(right_widget, stretch=1) + + self.timer = QtCore.QTimer() + self.timer.timeout.connect(self.update_display) + self.timer.start(33) + + def create_waveform_panel(self): + panel = QtWidgets.QWidget() + layout = QtWidgets.QVBoxLayout(panel) + layout.setSpacing(8) + + wave_ctrl = QtWidgets.QFrame() + wave_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #3b82f6; border-radius: 4px; }""") + wave_layout = QtWidgets.QHBoxLayout(wave_ctrl) + wave_layout.setContentsMargins(12, 6, 12, 6) + + title = QtWidgets.QLabel("WAVEFORM MONITOR (30Hz)") + title.setStyleSheet("color: #60a5fa; font-weight: bold; font-size: 13px;") + + self.wave_combo = QtWidgets.QComboBox() + self.wave_combo.addItems([ + "/cb_left_hand_matrix_touch_mass", + "/cb_right_hand_matrix_touch_mass" + ]) + self.wave_combo.setStyleSheet(""" + QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; } + QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #3b82f6; } + """) + self.wave_combo.currentTextChanged.connect(self.switch_wave_topic) + + wave_layout.addWidget(title) + wave_layout.addWidget(self.wave_combo) + wave_layout.addStretch() + layout.addWidget(wave_ctrl) + + self.wave_widget = pg.GraphicsLayoutWidget() + self.wave_widget.setBackground('#0f172a') + layout.addWidget(self.wave_widget, stretch=1) + + self.wave_plots = {} + self.wave_curves = {} + + for i, finger in enumerate(self.fingers_mass): + p = self.wave_widget.addPlot(row=i, col=0) + p.setMenuEnabled(False) + p.setMouseEnabled(x=False, y=False) + p.setYRange(0, 4500) + p.setXRange(0, 100) + p.showGrid(x=True, y=True, alpha=0.3) + p.getAxis('left').setTextPen('#64748b') + p.getAxis('bottom').setTextPen('#64748b') + + color = QtGui.QColor(*self.finger_colors[i]) + display_name = finger.replace('_mass', '').upper() + p.setTitle(f"[ {display_name} ]", color=color, size='11pt') + + if i == 4: + p.getAxis('bottom').setLabel('Time', color='#64748b') + else: + p.getAxis('bottom').setStyle(showValues=False) + + pen = pg.mkPen(color=self.finger_colors[i], width=2.5) + curve = p.plot(pen=pen) + self.wave_curves[finger] = curve + + text = pg.TextItem(text="0", color=(255,255,255), anchor=(1, 0.5)) + text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold)) + p.addItem(text) + self.wave_curves[finger + '_text'] = text + + return panel + + def create_heatmap_panel(self): + panel = QtWidgets.QWidget() + layout = QtWidgets.QVBoxLayout(panel) + layout.setSpacing(8) + + heat_ctrl = QtWidgets.QFrame() + heat_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #f43f5e; border-radius: 4px; }""") + heat_layout = QtWidgets.QHBoxLayout(heat_ctrl) + heat_layout.setContentsMargins(12, 6, 12, 6) + + title = QtWidgets.QLabel("PRESSURE MATRIX (12×6)") + title.setStyleSheet("color: #f43f5e; font-weight: bold; font-size: 13px;") + + # 单位标签 + unit_label = QtWidgets.QLabel("Max Value (g)") + unit_label.setStyleSheet("color: #94a3b8; font-size: 11px; border: none; background: transparent;") + heat_layout.addWidget(unit_label) + heat_layout.addSpacing(10) + + self.matrix_combo = QtWidgets.QComboBox() + self.matrix_combo.addItems([ + "/cb_left_hand_matrix_touch", + "/cb_right_hand_matrix_touch" + ]) + self.matrix_combo.setStyleSheet(""" + QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; } + QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #f43f5e; } + """) + self.matrix_combo.currentTextChanged.connect(self.switch_matrix_topic) + + heat_layout.addWidget(title) + heat_layout.addWidget(self.matrix_combo) + heat_layout.addStretch() + layout.addWidget(heat_ctrl) + + heat_container = QtWidgets.QWidget() + heat_grid = QtWidgets.QGridLayout(heat_container) + heat_grid.setSpacing(10) + heat_grid.setContentsMargins(5, 5, 5, 5) + + self.matrix_plots = {} + self.matrix_images = {} + self.matrix_peaks = {} + + positions = [ + ('thumb_matrix', 0, 0), ('index_matrix', 0, 1), ('middle_matrix', 0, 2), + ('ring_matrix', 1, 0), ('little_matrix', 1, 1) + ] + + self.colormap = pg.colormap.get('plasma') + + for idx, (finger, row, col) in enumerate(positions): + container = QtWidgets.QWidget() + vbox = QtWidgets.QVBoxLayout(container) + vbox.setSpacing(2) + vbox.setContentsMargins(0, 0, 0, 0) + + display_name = finger.replace('_matrix', '').upper() + name_label = QtWidgets.QLabel(display_name) + name_label.setAlignment(QtCore.Qt.AlignCenter) + color_hex = '#{:02x}{:02x}{:02x}'.format(*self.finger_colors[idx]) + name_label.setStyleSheet(f"color: {color_hex}; font-weight: bold; font-size: 12px;") + vbox.addWidget(name_label) + + # 固定比例显示 12×6 (高:宽 = 2:1) + view = pg.PlotWidget() + view.setMenuEnabled(False) + view.setMouseEnabled(x=False, y=False) + view.setAspectLocked(True, ratio=6/12) # X:Y = 1:2 + view.setMaximumSize(240, 280) + view.setMinimumSize(120, 160) + view.setBackground('#0f172a') + view.hideAxis('left') + view.hideAxis('bottom') + + img = pg.ImageItem() + img.setLookupTable(self.colormap.getLookupTable()) + img.setImage(np.zeros((12, 6)), levels=[0, 100]) + view.addItem(img) + + peak_text = pg.TextItem(text="", color=(255,255,255), anchor=(0.5, 0.5)) + peak_text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold)) + view.addItem(peak_text) + + vbox.addWidget(view, stretch=1) + heat_grid.addWidget(container, row, col) + + self.matrix_plots[finger] = view + self.matrix_images[finger] = img + self.matrix_peaks[finger] = peak_text + + # 颜色条 + legend_widget = QtWidgets.QWidget() + legend_layout = QtWidgets.QVBoxLayout(legend_widget) + legend_layout.setAlignment(QtCore.Qt.AlignCenter) + + lbl_max = QtWidgets.QLabel("MAX") + lbl_max.setStyleSheet("color: #fbbf24; font-size: 9px;") + lbl_max.setAlignment(QtCore.Qt.AlignCenter) + + gradient = pg.GradientWidget(orientation='right') + gradient.setMaximumWidth(25) + gradient.setMaximumHeight(180) + gradient.setColorMap(self.colormap) + + lbl_min = QtWidgets.QLabel("0") + lbl_min.setStyleSheet("color: #3b82f6; font-size: 9px;") + lbl_min.setAlignment(QtCore.Qt.AlignCenter) + + legend_layout.addWidget(lbl_max) + legend_layout.addWidget(gradient, stretch=1) + legend_layout.addWidget(lbl_min) + + heat_grid.addWidget(legend_widget, 1, 2) + + layout.addWidget(heat_container, stretch=1) + return panel + + def setup_ros(self): + self.get_logger().info("Setting up ROS...") + self.subscribe_waveform("/cb_left_hand_matrix_touch_mass") + self.subscribe_matrix("/cb_left_hand_matrix_touch") + + def subscribe_waveform(self, topic): + if self.wave_sub: + self.destroy_subscription(self.wave_sub) + self.wave_sub = self.create_subscription(String, topic, self.wave_callback, self.qos_profile) + self.get_logger().info(f"WAVEFORM: {topic}") + + def subscribe_matrix(self, topic): + if self.matrix_sub: + self.destroy_subscription(self.matrix_sub) + self.matrix_sub = self.create_subscription(String, topic, self.matrix_callback, self.qos_profile) + self.get_logger().info(f"MATRIX: {topic}") + + def switch_wave_topic(self, topic): + self.subscribe_waveform(topic) + for f in self.fingers_mass: + self.wave_data[f] = np.zeros(100) + self.current_wave_values[f] = 0.0 + + def switch_matrix_topic(self, topic): + self.subscribe_matrix(topic) + for f in self.fingers_matrix: + self.matrix_data[f] = np.zeros((12, 6)) + self.matrix_shapes[f] = (12, 6) + self.data_received[f] = False + + def wave_callback(self, msg): + try: + data = json.loads(msg.data) + for finger in self.fingers_mass: + if finger in data: + self.current_wave_values[finger] = float(data[finger]) + except Exception as e: + pass + + def matrix_callback(self, msg): + """处理 12×6 矩阵数据""" + try: + data = json.loads(msg.data) + + for finger in self.fingers_matrix: + if finger not in data: + continue + + arr = np.array(data[finger], dtype=np.float32) + + if arr.ndim == 2: + rows, cols = arr.shape + self.data_received[finger] = True + self.matrix_data[finger] = arr + self.matrix_shapes[finger] = (rows, cols) + + # ROS2 节流日志 + current_time = self.get_clock().now() + if self.last_matrix_log_time[finger] is None or \ + (current_time - self.last_matrix_log_time[finger]).nanoseconds > 3e9: + self.last_matrix_log_time[finger] = current_time + self.get_logger().info(f"{finger}: {rows}×{cols}, max={arr.max():.1f}") + + except Exception as e: + self.get_logger().error(f"Matrix error: {str(e)}") + + def update_display(self): + # 更新波形图 + for finger in self.fingers_mass: + self.wave_data[finger] = np.roll(self.wave_data[finger], -1) + self.wave_data[finger][-1] = self.current_wave_values[finger] + self.wave_curves[finger].setData(self.wave_data[finger]) + + val = self.current_wave_values[finger] + self.wave_curves[finger + '_text'].setText(f"{val:.0f}") + self.wave_curves[finger + '_text'].setPos(99, val) + + # 更新热力图 - 修复翻转问题 + for finger in self.fingers_matrix: + data = self.matrix_data[finger] + rows, cols = self.matrix_shapes[finger] # 12×6 + + if not self.data_received[finger]: + continue + + img = self.matrix_images[finger] + view = self.matrix_plots[finger] + peak_text = self.matrix_peaks[finger] + + max_val = data.max() + + if max_val > 0: + display_data = data / max_val if max_val > 1 else data + else: + display_data = data + + # **关键修复**: 垂直翻转，使第0行(ROS数据第1行)显示在顶部 + display_data_flipped = np.flipud(display_data) + + # 显示翻转后的数据 (12×6) + img.setImage(display_data_flipped, autoLevels=False, levels=[0, 1]) + + # 设置视图范围 + view.setRange(xRange=(-0.5, cols-0.5), yRange=(-0.5, rows-0.5)) + + # 显示数值：矩阵中的最大压力值，单位g(克) + if max_val > 0: + # 找到原始数据中的最大值位置 + max_idx = np.unravel_index(np.argmax(data), data.shape) + + # 峰值显示格式: 最大值 + 单位 + peak_text.setText(f"{max_val:.0f}") + + # 坐标转换：因为图像翻转了，y坐标也要翻转 + # 原始行号row，在翻转后的图像中是 (rows-1-row) + flipped_row = (rows - 1) - max_idx[0] + peak_text.setPos(max_idx[1], flipped_row) + else: + peak_text.setText("0") + +def signal_handler(sig, frame): + QtWidgets.QApplication.quit() + sys.exit(0) + +def main(args=None): + rclpy.init(args=args) + + app = QtWidgets.QApplication(sys.argv) + app.setStyle('Fusion') + + signal.signal(signal.SIGINT, signal_handler) + + gui = PressureDiagram() + gui.show() + + # ROS2 spin 在 Qt 定时器中处理 + def ros_spin(): + rclpy.spin_once(gui, timeout_sec=0) + + ros_timer = QtCore.QTimer() + ros_timer.timeout.connect(ros_spin) + ros_timer.start(1) # ~1000Hz + + sys.exit(app.exec_()) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/pressure_diagram/resource/pressure_diagram b/src/pressure_diagram/resource/pressure_diagram new file mode 100644 index 0000000..e69de29 diff --git a/src/pressure_diagram/setup.cfg b/src/pressure_diagram/setup.cfg new file mode 100644 index 0000000..0a18512 --- /dev/null +++ b/src/pressure_diagram/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pressure_diagram +[install] +install_scripts=$base/lib/pressure_diagram diff --git a/src/pressure_diagram/setup.py b/src/pressure_diagram/setup.py new file mode 100644 index 0000000..7c61879 --- /dev/null +++ b/src/pressure_diagram/setup.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import os +from glob import glob +from setuptools import find_packages, setup +package_name = 'pressure_diagram' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='linkerhand', + maintainer_email='linkerhand@todo.todo', + description='ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand', + license='Apache-2.0', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'pressure_diagram = pressure_diagram.pressure_diagram:main', + ], + }, +) diff --git a/src/pressure_diagram/test/test_copyright.py b/src/pressure_diagram/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/pressure_diagram/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/pressure_diagram/test/test_flake8.py b/src/pressure_diagram/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/pressure_diagram/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/pressure_diagram/test/test_pep257.py b/src/pressure_diagram/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/pressure_diagram/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'