mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-03-24 09:39:17 +00:00
48e13a7b4d
- HostNode 直接订阅 /joint_states (JointStateMsg)，绕过 JointRepublisher 中间人 - 新增 resource_pose 订阅，实现资源夹取跟随 (gripper attach/detach) - 吞吐优化：死区过滤 (1e-4 rad)、抑频 (~20Hz)、增量 resource_poses - JointRepublisher 修复 str→json.dumps (E1) - communication.py 新增 publish_joint_state 抽象方法 - ws_client.py 实现 push_joint_state action 发送 - 57 项测试覆盖：关节分组、资源跟随、同类型多设备、优化行为 Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
64 lines
1.9 KiB
Python
64 lines
1.9 KiB
Python
import uuid
|
|
import rclpy,json
|
|
from rclpy.node import Node
|
|
from sensor_msgs.msg import JointState
|
|
from std_msgs.msg import String
|
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
|
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
|
|
|
|
class JointRepublisher(BaseROS2DeviceNode):
|
|
def __init__(self,device_id, registry_name, resource_tracker, **kwargs):
|
|
super().__init__(
|
|
driver_instance=self,
|
|
device_id=device_id,
|
|
registry_name=registry_name,
|
|
status_types={},
|
|
action_value_mappings={},
|
|
hardware_interface={},
|
|
print_publish=False,
|
|
resource_tracker=resource_tracker,
|
|
device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
|
|
)
|
|
|
|
# print('-'*20,device_id)
|
|
self.joint_repub = self.create_publisher(String,f'joint_state_repub',10)
|
|
# 创建订阅者
|
|
self.create_subscription(
|
|
JointState,
|
|
'/joint_states',
|
|
self.listener_callback,
|
|
10,
|
|
callback_group=ReentrantCallbackGroup()
|
|
)
|
|
self.msg = String()
|
|
|
|
def listener_callback(self, msg:JointState):
|
|
|
|
try:
|
|
json_dict = {}
|
|
json_dict["name"] = list(msg.name)
|
|
json_dict["position"] = list(msg.position)
|
|
json_dict["velocity"] = list(msg.velocity)
|
|
json_dict["effort"] = list(msg.effort)
|
|
|
|
self.msg.data = json.dumps(json_dict)
|
|
self.joint_repub.publish(self.msg)
|
|
# print('-'*20)
|
|
# print(self.msg.data)
|
|
|
|
except Exception as e:
|
|
print(e)
|
|
|
|
|
|
def main():
|
|
|
|
rclpy.init()
|
|
subscriber = JointRepublisher()
|
|
rclpy.spin(subscriber)
|
|
subscriber.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|