ROS2-Python-Skript zum Veröffentlichen benutzerdefinierter Joint States
Das folgende Skript generiert sinusförmige Joint States und veröffentlicht sie auf ein ROS2-Topic mit Python.
Die names- und joints-Variablen variieren je nach Roboter, das folgende Beispiel ist für einen 7-DoF-Roboter + einen End-Effector-DOF (Franka Research 3).
sinusoidal_joint_publisher.py
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import math
from time import time
class SinusoidalJointPublisher(Node):
def __init__(self):
super().__init__('sinusoidal_joint_publisher')
self.publisher = self.create_publisher(JointState, 'joint_states', 10)
self.timer = self.create_timer(0.1, self.timer_callback) # 10 Hz
self.start_time = time()
def timer_callback(self):
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.name = ['joint1']
# Calculate sinusoidal position (amplitude = 1, frequency = 0.5 Hz)
t = time() - self.start_time
position = math.sin(2 * math.pi * 0.5 * t)
msg.name = [
"fr3_joint1",
"fr3_joint2",
"fr3_joint3",
"fr3_joint4",
"fr3_joint5",
"fr3_joint6",
"fr3_joint7",
"fr3_finger_joint1",
"fr3_finger_joint2",
]
msg.position = [
0.0,
0.5,
0.5,
0.5,
position,
0.5,
0.5,
-0.05,
-0.05,
]
msg.velocity = [0.0]
msg.effort = [0.0]
self.publisher.publish(msg)
self.get_logger().info(f'Publishing joint position: {position}')
def main(args=None):
rclpy.init(args=args)
node = SinusoidalJointPublisher()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()If this post helped you, please consider buying me a coffee or donating via PayPal to support research & publishing of new posts on TechOverflow