admin/linkerhand_retarget_ros2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

三机位标定

Browse Source
This commit is contained in:
lxp
2026-08-05 18:32:51 +08:00
parent 05634f5472
commit 5d206bcb73
4 changed files with 0 additions and 2046 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/linker_hand_ros2_sdk/launch/test.bak
-31
View File
@@ -1,31 +0,0 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='linker_hand_ros2_sdk',
executable='linker_hand_sdk',
name='linker_hand_sdk_left',
output='screen',
parameters=[{
'hand_type': 'left',
'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
'is_touch': True, # 是否带有压力传感器
'can': 'can0', # 这里需要修改为实际的CAN总线名称
}],
),
Node(
package='linker_hand_ros2_sdk',
executable='linker_hand_sdk',
name='linker_hand_sdk_right',
output='screen',
parameters=[{
'hand_type': 'right',
'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
'is_touch': True, # 是否带有压力传感器
'can': 'can0', # 这里需要修改为实际的CAN总线名称
}],
),
])
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak
-444
View File
@@ -1,444 +0,0 @@
#!/usr/bin/env python3
import os
import time
from pymodbus.client import ModbusSerialClient
from typing import List, Dict
import numpy as np
_INTERVAL = 0.006 # 8 ms
class LinkerHandL6RS485:
"""L6机械手 Modbus-RTU 控制类"""
# 6个关节名称
JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch",
"middle_pitch", "ring_pitch", "little_pitch"]
# 手指名称
FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
"""
初始化L6机械手
hand_id: 右手0x27(39), 左手0x28(40)
modbus_port: 串口设备路径
baudrate: 波特率，固定115200
"""
self.slave = hand_id
self.cli = ModbusSerialClient(
port=modbus_port,
baudrate=baudrate,
bytesize=8,
parity="N",
stopbits=1,
timeout=0.05
)
# pymodbus 3.5.1 需要显式连接
self.connected = self.cli.connect()
if not self.connected:
raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
def _read_input_registers(self, address: int, count: int) -> List[int]:
"""读取输入寄存器"""
time.sleep(_INTERVAL)
result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
if result.isError():
raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
return result.registers
def _write_register(self, address: int, value: int):
"""写入单个寄存器"""
time.sleep(_INTERVAL)
result = self.cli.write_register(address=address, value=value, slave=self.slave)
if result.isError():
raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
def _write_registers(self, address: int, values: List[int]):
"""写入多个寄存器"""
time.sleep(_INTERVAL)
result = self.cli.write_registers(address=address, values=values, slave=self.slave)
if result.isError():
raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
# --------------------------------------------------
# 基础读取接口
# --------------------------------------------------
def read_angles(self) -> List[int]:
"""读取6个关节角度 (输入寄存器 0-5)"""
return self._read_input_registers(0, 6)
def read_torques(self) -> List[int]:
"""读取6个关节转矩 (输入寄存器 6-11)"""
return self._read_input_registers(6, 6)
def read_speeds(self) -> List[int]:
"""读取6个关节速度 (输入寄存器 12-17)"""
return self._read_input_registers(12, 6)
def read_temperatures(self) -> List[int]:
"""读取6个关节温度 (输入寄存器 18-23)"""
return self._read_input_registers(18, 6)
def read_error_codes(self) -> List[int]:
"""读取6个关节错误码 (输入寄存器 24-29)"""
return self._read_input_registers(24, 6)
# --------------------------------------------------
# 压力传感器接口
# --------------------------------------------------
# def _pressure(self, finger: int) -> List[int]:
# """内部：选手指 → 读压力数据"""
# # 选择手指 (保持寄存器 36)
# self._write_register(36, finger)
# time.sleep(_INTERVAL)
# # 读取压力数据 (输入寄存器 52-122)
# return np.array(self._read_input_registers(52, 71))
def _pressure(self, finger: int) -> np.ndarray:
"""
6x12 (72点) 矩阵尺寸。
Modbus 地址 60/62。
"""
rows = 12 # 12 行
cols = 6 # 6 列
finger_size = rows * cols # 72 个数据点
# modbus 地址和计数
write_address = 60 # 写入手指选择
read_address = 62 # 读取压力数据
read_count = 96 # 读取 96 个寄存器
skip_count = 10 # 跳过前 10 个校验点
# 0. 参数校验和手指写入值确定
if finger < 1 or finger > 5:
raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
finger_write_value = finger
# 1. 写入手指选择寄存器 (地址 60)
time.sleep(0.008)
wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
if wrsp.isError():
raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
# 写入后等待片刻
time.sleep(0.008)
# 2. 读取地址 62 的数据
rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
if rrsp.isError():
raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
registers_16bit: List[int] = rrsp.registers
# 3. 核心数据处理
# a. 提取低 8 位数据 (得到 96 个 8 位数据点)
final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
# b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
# c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
start_idx = 0
end_idx = finger_size # 72
finger_data_flat = effective_data[start_idx:end_idx]
# d. 验证数据长度
if finger_data_flat.size != finger_size:
raise ValueError(
f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
)
# e. 重塑为二维矩阵 (12 行 6 列)
finger_matrix = finger_data_flat.reshape((rows, cols))
return finger_matrix
def read_pressure_thumb(self) -> np.ndarray:
"""读取大拇指压力数据"""
return np.array(self._pressure(1), dtype=np.uint8)
def read_pressure_index(self) -> np.ndarray:
"""读取食指压力数据"""
return np.array(self._pressure(2), dtype=np.uint8)
def read_pressure_middle(self) -> np.ndarray:
"""读取中指压力数据"""
return np.array(self._pressure(3), dtype=np.uint8)
def read_pressure_ring(self) -> np.ndarray:
"""读取无名指压力数据"""
return np.array(self._pressure(4), dtype=np.uint8)
def read_pressure_little(self) -> np.ndarray:
"""读取小拇指压力数据"""
return np.array(self._pressure(5), dtype=np.uint8)
# --------------------------------------------------
# 版本信息接口
# --------------------------------------------------
def read_versions(self) -> Dict[str, int]:
"""读取版本信息 (输入寄存器 148-155)"""
result = self._read_input_registers(148, 8)
return {
"hand_freedom": result[0],
"hand_version": result[1],
"hand_number": result[2],
"hand_direction": result[3],
"software_version_major": result[4],
"software_version_minor": result[5] if len(result) > 5 else 0,
"software_version_revision": result[6] if len(result) > 6 else 0,
"hardware_version": result[7] if len(result) > 7 else 0
}
# --------------------------------------------------
# 写入接口
# --------------------------------------------------
def write_angles(self, vals: List[int]):
"""设置6个关节角度 (保持寄存器 0-5)"""
vals = [int(x) for x in vals]
if not self.is_valid_6xuint8(vals):
raise ValueError("需要6个0-255的整数")
self._write_registers(0, vals)
def write_torques(self, vals: List[int]):
"""设置6个关节转矩 (保持寄存器 6-11)"""
vals = [int(x) for x in vals]
if not self.is_valid_6xuint8(vals):
raise ValueError("需要6个0-255的整数")
self._write_registers(6, vals)
def write_speeds(self, vals: List[int]):
"""设置6个关节速度 (保持寄存器 12-17)"""
vals = [int(x) for x in vals]
if not self.is_valid_6xuint8(vals):
raise ValueError("需要6个0-255的整数")
self._write_registers(12, vals)
# --------------------------------------------------
# 上下文管理
# --------------------------------------------------
def close(self):
"""关闭连接"""
if self.connected:
self.cli.close()
self.connected = False
def __enter__(self):
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.close()
# --------------------------------------------------
# API固定接口函数
# --------------------------------------------------
def is_valid_6xuint8(self, lst) -> bool:
"""验证6个0-255的整数列表"""
if len(lst) != 6:
return False
return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
def set_joint_positions(self, joint_angles=None):
"""设置关节位置"""
joint_angles = joint_angles or [0] * 6
self.write_angles(joint_angles)
def set_speed(self, speed=None):
"""设置速度"""
speed = speed or [200] * 6
self.write_speeds(speed)
def set_torque(self, torque=None):
"""设置扭矩"""
torque = torque or [200] * 6
self.write_torques(torque)
def set_current(self, current=None):
"""设置电流 (L6不支持)"""
print("当前L6不支持设置电流", flush=True)
def get_version(self) -> list:
"""获取版本信息"""
versions = self.read_versions()
return [
versions.get("hand_freedom", 0),
versions.get("hand_version", 0),
versions.get("hand_number", 0),
versions.get("hand_direction", 0),
versions.get("software_version_major", 0),
versions.get("hardware_version", 0)
]
def get_current(self):
"""获取电流 (L6不支持)"""
print("当前L6不支持获取电流", flush=True)
return []
def get_state(self) -> list:
"""获取关节状态"""
return self.read_angles()
def get_state_for_pub(self) -> list:
return self.get_state()
def get_current_status(self) -> list:
return self.get_state()
def get_speed(self) -> list:
"""获取当前速度"""
return self.read_speeds()
def get_joint_speed(self) -> list:
return self.get_speed()
def get_touch_type(self) -> int:
"""获取压感类型 (2=矩阵式)"""
return 2
def get_normal_force(self) -> list:
"""获取压感数据：点式"""
return [-1] * 5
def get_tangential_force(self) -> list:
"""获取压感数据：点式"""
return [-1] * 5
def get_approach_inc(self) -> list:
"""获取压感数据：点式"""
return [-1] * 5
def get_touch(self) -> list:
return [-1] * 5
def get_thumb_matrix_touch(self,sleep_time=0):
return self._pressure(1)
def get_index_matrix_touch(self,sleep_time=0):
return self._pressure(2)
def get_middle_matrix_touch(self,sleep_time=0):
return self._pressure(3)
def get_ring_matrix_touch(self,sleep_time=0):
return self._pressure(4)
def get_little_matrix_touch(self,sleep_time=0):
return self._pressure(5)
def get_matrix_touch(self) -> list:
"""获取压感数据：矩阵式"""
return [self._pressure(1), self._pressure(2), self._pressure(3),
self._pressure(4), self._pressure(5)]
def get_matrix_touch_v2(self) -> list:
"""获取压感数据：矩阵式"""
return self.get_matrix_touch()
def get_torque(self) -> list:
"""获取当前扭矩"""
return self.read_torques()
def get_temperature(self) -> list:
"""获取当前电机温度"""
return self.read_temperatures()
def get_fault(self) -> list:
"""获取当前电机故障码"""
return self.read_error_codes()
def get_serial_number(self):
return [0] * 6
# --------------------------------------------------
# 便捷方法
# --------------------------------------------------
def relax(self):
"""所有手指伸直"""
self.set_joint_positions([255] * 6)
def fist(self):
"""所有手指握拳"""
self.set_joint_positions([0] * 6)
def dump_status(self):
"""打印状态信息"""
print("=" * 50)
print("L6机械手状态信息")
print("=" * 50)
try:
# 关节状态
angles = self.read_angles()
torques = self.read_torques()
speeds = self.read_speeds()
temps = self.read_temperatures()
errors = self.read_error_codes()
print("关节状态:")
for i, name in enumerate(self.JOINT_NAMES):
print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
# 版本信息
versions = self.read_versions()
print("\n版本信息:")
for key, value in versions.items():
print(f" {key:20s}: {value}")
# 压力传感器测试
print("\n压力传感器测试:")
thumb_pressure = self.read_pressure_thumb()
print(f"大拇指压力数据长度: {len(thumb_pressure)}")
except Exception as e:
print(f"读取状态时出错: {e}")
print("=" * 50)
# ------------------- 演示程序 -------------------
if __name__ == "__main__":
# 使用示例
try:
with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
print("连接成功!")
# 打印状态信息
hand.dump_status()
# 测试基本控制
print("\n测试控制功能...")
print("伸直手指...")
hand.relax()
time.sleep(2)
print("握拳...")
hand.fist()
time.sleep(2)
print("恢复伸直...")
hand.relax()
# 测试压力传感器
print("\n测试压力传感器...")
thumb_matrix = hand.get_thumb_matrix_touch()
print(f"大拇指压力数据: {len(thumb_matrix)}个点")
# 获取所有手指压力数据
all_matrices = hand.get_matrix_touch()
for i, name in enumerate(hand.FINGER_NAMES):
matrix = all_matrices[i]
print(f"{name}手指压力数据长度: {len(matrix)}")
except Exception as e:
print(f"错误: {e}")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak
-1157
View File
File diff suppressed because it is too large Load Diff
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak
-414
View File
@@ -1,414 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
'''
编译: colcon build --symlink-install
启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
'''
from re import A
import rclpy,sys # ROS2 Python接口库
import time
import numpy as np
from rclpy.node import Node # ROS2 节点类
from rclpy.clock import Clock
from std_msgs.msg import String, Header, Float32MultiArray
from sensor_msgs.msg import JointState, PointCloud2, PointField
import time, json, threading
from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
class LinkerHand(Node):
def __init__(self, name):
super().__init__(name)
# 声明参数（带默认值）
self.declare_parameter('hand_type', 'left')
self.declare_parameter('hand_joint', 'L6')
self.declare_parameter('is_touch', False)
self.declare_parameter('can', 'can0')
self.declare_parameter('modbus', "None")
# ros时间获取
self.stamp_clock = Clock()
# 获取参数值
self.hand_type = self.get_parameter('hand_type').value
self.hand_joint = self.get_parameter('hand_joint').value
self.is_touch = self.get_parameter('is_touch').value
self.can = self.get_parameter('can').value
self.modbus = self.get_parameter('modbus').value
self.sdk_v = 2
self.sleep_time = 0.005
self.cmd_lock = False
self.last_hand_post_cmd = None # 最新手指位置命令
self.last_hand_vel_cmd = None # 最新手指速度命令
self.last_hand_eff_cmd = None # 最新手指力矩命令
self.last_hand_state = [-1] * 10
self.last_hand_vel = [-1] * 10
self.force = [[-1] * 5] * 4
self.matrix_dic = {
"stamp":{
"sec": 0,
"nanosec": 0,
},
"thumb_matrix":[[-1] * 6 for _ in range(12)],
"index_matrix":[[-1] * 6 for _ in range(12)],
"middle_matrix":[[-1] * 6 for _ in range(12)],
"ring_matrix":[[-1] * 6 for _ in range(12)],
"little_matrix":[[-1] * 6 for _ in range(12)]
}
# 压感矩阵合值，单位g 克
self.matrix_mass_dic = {
"stamp":{
"secs": 0,
"nsecs": 0,
},
"thumb_mass":[-1],
"index_mass":[-1],
"middle_mass":[-1],
"ring_mass":[-1],
"little_mass":[-1]
}
self.last_hand_info = {
"version": [-1], # Dexterous hand version number
"hand_joint": self.hand_joint, # Dexterous hand joint type
"speed": [-1] * 10, # Current speed threshold of the dexterous hand
"current": [-1] * 10, # Current of the dexterous hand
"fault": [-1] * 10, # Current fault of the dexterous hand
"motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
"torque": [-1] * 10, # Current torque of the dexterous hand
"is_touch":self.is_touch,
"touch_type": -1,
"finger_order": None # Finger motor order
}
self.version = []
self.touch_type = -1
self.hz = 1.0/60.0
self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
self._init_hand()
time.sleep(1)
self.run_count = 0 # 计数器，用于记录运行次数
self.timer = self.create_timer(0.01, self.run) # 100 Hz
self.thread_pub_state = threading.Thread(target=self.pub_state)
self.thread_pub_state.daemon = True
self.thread_pub_state.start()
def _init_hand(self):
self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
time.sleep(0.1)
self.touch_type = self.api.get_touch_type()
self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
if self.is_touch == True:
if self.touch_type > 1:
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
elif self.touch_type != -1:
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
else:
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
self.is_touch = False
self.embedded_version = self.api.get_embedded_version()
pose = None
torque = [200, 200, 200, 200, 200]
speed = [200, 250, 250, 250, 250]
if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
pose = [200, 255, 255, 255, 255, 180]
torque = [250, 250, 250, 250, 250, 250]
# O6 最大速度阈值
speed = [200, 250, 250, 250, 250, 250]
elif self.hand_joint == "L7":
# The data length of L7 is 7, reinitialize here
pose = [255, 200, 255, 255, 255, 255, 180]
torque = [250, 250, 250, 250, 250, 250, 250]
speed = [120, 250, 250, 250, 250, 250, 250]
elif self.hand_joint == "L10":
torque = [255] * 10
pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
elif self.hand_joint == "L20":
pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
elif self.hand_joint == "L21":
pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
elif self.hand_joint == "L25":
pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
if pose is not None:
for i in range(1):
self.api.set_speed(speed=speed)
time.sleep(0.1)
self.api.set_torque(torque=torque)
time.sleep(0.1)
self.api.finger_move(pose=pose)
time.sleep(0.1)
def list_check(self,pose):
if isinstance(pose, list) == False:
return False
if len(self.last_hand_post_cmd) != len(pose):
return False
return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
def hand_control_cb(self, msg):
if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
self.last_hand_post_cmd = msg.position
if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
self.last_hand_vel_cmd = msg.velocity
if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
self.last_hand_eff_cmd = msg.effort
def run(self):
if self.sdk_v == 1:
self.sleep_time = 0.009
if self.hand_state_pub.get_subscription_count() > 0:
# 优先获取手指状态并且发布
self.last_hand_state = self.api.get_state()
time.sleep(0.003)
self.last_hand_vel = self.api.get_joint_speed()
time.sleep(0.002)
if self.cmd_lock == False:
if self.last_hand_post_cmd != None:
self.api.finger_move(pose=self.last_hand_post_cmd)
self.last_hand_post_cmd = None
if self.last_hand_vel_cmd != None:
vel = list(self.last_hand_vel_cmd)
if all(x == 0 for x in vel):
pass
else:
if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
speed = vel
self.api.set_joint_speed(speed=speed)
elif self.hand_joint == "L7" and len(vel) == 7:
speed = vel
self.api.set_joint_speed(speed=speed)
elif self.hand_joint == "L10" and len(vel) == 10:
speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
self.api.set_joint_speed(speed=speed)
elif self.hand_joint == "L20" and len(vel) == 20:
speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
self.api.set_joint_speed(speed=speed)
elif self.hand_joint == "L21" and len(vel) == 25:
speed = vel
self.api.set_joint_speed(speed=speed)
elif self.hand_joint == "L25" and len(vel) == 25:
speed = vel
self.api.set_joint_speed(speed=speed)
self.last_hand_vel_cmd = None
time.sleep(0.003)
if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
"""单点式压力传感器"""
self.force = self.api.get_force()
if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
"""矩阵式压力传感器"""
if self.run_count == 3:
self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
if self.run_count == 4:
self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
if self.run_count == 5:
self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
if self.run_count == 6:
self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
if self.run_count == 7:
self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
time.sleep(0.005)
if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
"""手部信息"""
self.last_hand_info = {
"version": self.embedded_version, # Dexterous hand version number
"hand_joint": self.hand_joint, # Dexterous hand joint type
"speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
"current": self.api.get_current(), # Current of the dexterous hand
"fault": self.api.get_fault(), # Current fault of the dexterous hand
"motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
"torque": self.api.get_torque(), # Current torque of the dexterous hand
"is_touch":self.is_touch,
"touch_type": self.touch_type,
"finger_order": self.api.get_finger_order() # Finger motor order
}
if self.run_count == 9:
self.run_count = 0
self.run_count += 1
time.sleep(0.003)
def pub_state(self):
while True:
if self.hand_state_pub.get_subscription_count() > 0:
msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
self.hand_state_pub.publish(msg)
if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
msg = Float32MultiArray()
msg.data = [float(val) for sublist in self.force for val in sublist]
self.touch_pub.publish(msg)
if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
# 发布矩阵压感数据JSON格式
self.pub_matrix_dic()
# 发布矩阵压感和值JSON格式
self.pub_matrix_mass(dic=self.matrix_dic)
# 发布矩阵压感点云格式
self.pub_matrix_point_cloud()
if self.hand_info_pub.get_subscription_count() > 0:
msg = String()
msg.data = json.dumps(self.last_hand_info)
self.hand_info_pub.publish(msg)
time.sleep(self.hz)
def pub_matrix_mass(self, dic):
"""发布矩阵数据合值 单位g 克 JSON格式"""
msg = String()
# 获取当前的 ROS 时间
current_time = self.stamp_clock.now()
# 提取 secs 和 nsecs
t_secs = current_time.to_msg().sec
t_nsecs = current_time.to_msg().nanosec
self.matrix_mass_dic["stamp"]["secs"] = t_secs
self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
self.matrix_mass_dic["unit"] = "g"
self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
msg.data = json.dumps(self.matrix_mass_dic)
self.matrix_touch_mass_pub.publish(msg)
def pub_matrix_point_cloud(self):
"""发布矩阵数据点云格式"""
tmp_dic = self.matrix_dic.copy()
del tmp_dic['stamp'] # 去掉时间戳字段
all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数
# 摊平到一维：360 个 float
flat_list = [v for frame in all_matrices for v in frame] # 360
flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
fields = [PointField(
name='val',
offset=0,
datatype=PointField.UINT8,
count=1
)]
pc = PointCloud2()
pc.header.stamp = self.stamp_clock.now().to_msg()
pc.header.frame_id = ''
pc.height = 1
pc.width = flat.size # 360
pc.fields = fields
pc.is_bigendian = False
pc.point_step = 1 # 1 个 float32
pc.row_step = pc.point_step * pc.width
pc.data = flat.tobytes() # 1440 字节
self.matrix_touch_pub_pc.publish(pc)
def pub_matrix_dic(self):
"""发布矩阵数据JSON格式"""
msg = String()
# 获取当前的 ROS 时间
current_time = self.stamp_clock.now()
# 提取 secs 和 nsecs
t_secs = current_time.to_msg().sec
t_nsecs = current_time.to_msg().nanosec
self.matrix_dic["stamp"]["secs"] = t_secs
self.matrix_dic["stamp"]["nsecs"] = t_nsecs
msg.data = json.dumps(self.matrix_dic)
self.matrix_touch_pub.publish(msg)
def joint_state_msg(self, pose,vel=[]):
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = self.get_clock().now().to_msg()
joint_state.name = self.api.get_finger_order()
joint_state.position = [float(x) for x in pose]
if len(vel) > 1:
joint_state.velocity = [float(x) for x in vel]
else:
joint_state.velocity = [0.0] * len(pose)
joint_state.effort = [0.0] * len(pose)
return joint_state
def hand_setting_cb(self,msg):
'''控制命令回调'''
data = json.loads(msg.data)
print(f"Received setting command: {data['setting_cmd']}",flush=True)
try:
if data["params"]["hand_type"] == "left":
hand = self.api
hand_left = True
elif data["params"]["hand_type"] == "right":
hand = self.api
hand_right = True
else:
print("Please specify the hand part to be set",flush=True)
return
self.cmd_lock = True
# Set maximum torque
if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
torque = list(data["params"]["torque"])
hand.set_torque(torque=torque)
if data["setting_cmd"] == "set_speed": # Set speed
if isinstance(data["params"]["speed"], list) == True:
speed = data["params"]["speed"]
hand.set_speed(speed=speed)
else:
ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
if data["setting_cmd"] == "clear_faults": # Clear faults
if hand_left == True and self.hand_joint == "L10" :
ColorMsg(msg=f"L10 left hand cannot clear faults")
elif hand_right == True and self.hand_joint == "L10" :
ColorMsg(msg=f"L10 right hand cannot clear faults")
else:
hand.clear_faults()
if data["setting_cmd"] == "get_faults": # Get faults
f = hand.get_fault()
ColorMsg(msg=f"Get faults: {f}")
if data["setting_cmd"] == "electric_current": # Get current
ColorMsg(msg=f"Get current: {hand.get_current()}")
if data["setting_cmd"] == "set_electric_current": # Set current
if isinstance(data["params"]["current"], list) == True:
hand.set_current(data["params"]["current"])
if data["setting_cmd"] == "show_fun_table": # Get faults
f = hand.show_fun_table()
except:
print("命令参数错误")
self.cmd_lock = False
finally:
self.cmd_lock = False
def close_can(self):
self.api.open_can.close_can(can=self.can)
sys.exit(0)
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":
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":
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)):
ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
node.sdk_v = 2
else:
ColorMsg(msg=f"SDK V1", color="green")
node.sdk_v = 1
rclpy.spin(node) # 主循环，监听 ROS 回调
except KeyboardInterrupt:
print("收到 Ctrl+C，准备退出...")
finally:
# node.close_can() # 关闭 CAN 或其他硬件资源
# node.destroy_node() # 销毁 ROS 节点
# rclpy.shutdown() # 关闭 ROS
print("程序已退出。")