GUI可视化页面，角度与速度曲线记录节点o6_joint_curve.py
This commit is contained in:
@@ -16,6 +16,7 @@ htmlcov/
|
||||
.ruff_cache/
|
||||
|
||||
# Runtime and calibration scratch files
|
||||
/logs/
|
||||
*.tmp
|
||||
*.log
|
||||
|
||||
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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('<Configure>', 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(
|
||||
"<Configure>",
|
||||
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()
|
||||
@@ -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)
|
||||
@@ -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))
|
||||
@@ -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,
|
||||
# }],
|
||||
# ),
|
||||
])
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>gui_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<exec_depend>linker_hand_ros2_sdk</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,3 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=61.0"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/gui_control
|
||||
[install]
|
||||
install_scripts=$base/lib/gui_control
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
@@ -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
|
||||
|
||||
@@ -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)):
|
||||
|
||||
@@ -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_<SN>.json containing joints.<name>.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()
|
||||
@@ -14,6 +14,9 @@
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-matplotlib</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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=[{
|
||||
|
||||
}],
|
||||
),
|
||||
|
||||
])
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>pressure_diagram</name>
|
||||
<version>0.0.0</version>
|
||||
<description>ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand</description>
|
||||
<maintainer email="linkerhand@todo.todo">linkerhand</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<!-- ROS2 dependencies -->
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
|
||||
<!-- Python dependencies -->
|
||||
<exec_depend>python3-pyqt5</exec_depend>
|
||||
<exec_depend>python3-pyqtgraph</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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()
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/pressure_diagram
|
||||
[install]
|
||||
install_scripts=$base/lib/pressure_diagram
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
Reference in New Issue
Block a user