OSC_ROS2/ros_osc/joint_info/pythonosc/joint_names_reader.py
Alexander Schaefer 98759f2bab AS: osc4py3
2025-03-14 09:37:10 +01:00

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()