UR control via Ros2 Humble Python node

[Icon45]

Hi everyone,

this isn't really an issue, but since there is no Disscussion-Section here I thought that I would ask my question here.

I'm trying to control the joints of an ur10 (in Gazebo) via a node. I construct a FollowJointTrajectory.Goal() message and I'm trying to send that via an ActionClient(self, FollowJointTrajectory, "joint_trajectory_controller).

Unfortunately I get stuck when I'm waiting for the server, which I thought was being spawned when launching the UR. Does anyone know what's going on and wants to help me?

So here' my code:

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.time import Duration

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.action import FollowJointTrajectory

class Ur10ManualJoints(Node):

    def __init__(self):
        super().__init__('Ur10ManualJoints')
        self.actionClient = ActionClient(self, FollowJointTrajectory, '/joint_trajectory_controller')

        self.execute()

    def sendTrajGoal(self, joint_namen, joint_pos, duration):
        zielMsg = FollowJointTrajectory.Goal()
        zielMsg.trajectory.joint_names = joint_namen

        pkt = JointTrajectoryPoint()
        pkt.positions = joint_pos
        pkt.time_from_start = duration

        zielMsg.trajectory.points.append(pkt)

        self.get_logger().info("waiting for server...")
        self.actionClient.wait_for_server()
        self.get_logger().info("sending Action Goal...")
        self.actionClient.send_goal_async(zielMsg) #vllt mit return
        self.get_logger().info("Action Goal sent!")

    def execute(self):
        jointNamen = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']

        jointPos = []

        for jointName in jointNamen:
            pos = input(f"Enter position for {jointName}:")
            jointPos.append(float(pos))

        duration = rclpy.time.Duration(seconds=5).to_msg()

        self.sendTrajGoal(jointNamen, jointPos, duration)

def main(args=None):
    rclpy.init(args=args)
    node = Ur10ManualJoints()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()