mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 04:51:10 +00:00
61 lines
1.7 KiB
Python
61 lines
1.7 KiB
Python
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,resource_tracker):
|
|
super().__init__(
|
|
driver_instance=self,
|
|
device_id=device_id,
|
|
status_types={},
|
|
action_value_mappings={},
|
|
hardware_interface={},
|
|
print_publish=False,
|
|
resource_tracker=resource_tracker,
|
|
)
|
|
|
|
# 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 = str(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()
|