mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 13:01:12 +00:00
52 lines
1.4 KiB
Python
52 lines
1.4 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
|
|
|
|
class JointRepublisher(Node):
|
|
def __init__(self,device_id):
|
|
super().__init__(device_id)
|
|
|
|
# print('-'*20,device_id)
|
|
self.joint_repub = self.create_publisher(String,f'/devices/{device_id}/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()
|