43 lines
1.3 KiB
Python
43 lines
1.3 KiB
Python
import rclpy
|
|
from rclpy.node import Node
|
|
from sensor_msgs.msg import JointState
|
|
from rclpy.executors import SingleThreadedExecutor
|
|
|
|
class JointStateReader(Node):
|
|
def __init__(self):
|
|
super().__init__('JointStateReader')
|
|
self.sub = self.create_subscription(JointState, 'joint_states', self.sub_callback, 1)
|
|
self.joint_names = []
|
|
self.got_msg = False
|
|
|
|
def sub_callback(self, msg):
|
|
"""Callback to read joint names from the first received message."""
|
|
if not self.joint_names:
|
|
self.got_msg=True
|
|
self.joint_names = list(msg.name)
|
|
|
|
def joint_names():
|
|
"""Fetch joint names using an existing ROS2 context and ensure proper cleanup."""
|
|
rclpy.init()
|
|
node = JointStateReader()
|
|
executor = SingleThreadedExecutor()
|
|
executor.add_node(node)
|
|
while not node.got_msg:
|
|
executor.spin_once(timeout_sec=0)
|
|
joint_list = node.joint_names # Store names before shutting down
|
|
|
|
node.destroy_node() # Properly destroy the node
|
|
rclpy.shutdown() # Shutdown ROS2 context to free memory
|
|
|
|
return joint_list # Return the joint names safely
|
|
|
|
def main():
|
|
"""Main function to fetch and print joint names."""
|
|
joints = joint_names()
|
|
|
|
for i, joint_name in enumerate(joints, start=1):
|
|
print(f"Joint No. {i}: {joint_name}")
|
|
|
|
if __name__ == '__main__':
|
|
main()
|