Webots blocks moveit action server when other (Supervisor) nodes run

[alexandrosnic]

Describe the Bug

I run Webots R2022a, with ROS2 on an environment with a universal robot (UR). For this, I was based on the official webots_ros2_universal_robot package, but I integrated the newly released UR ROS2 description.

I run my environment, with some nodes running on the background (for example a topic that publishes the positions of the objects).
I run the nodes with "ros2 run" (since integrating them as a plugin in the urdf, resulted in not compiling them. This may be another bug but I couldn't verify yet).

All works good. When I run a moveit2 action client, to execute a pick command, however, moveit stays until planning and does not execute.
When I run moveit without having the other nodes ran yet, moveit works like a charm.
I believe that it has to do with threading or callbacks handling being blocked by the other nodes.

To verify, I ran it also with the official moveit_demo_launch.py from the webots_ros2_universal_robot package
A simple node for reproduction could be:

import rclpy
from controller import Connector
from controller import Robot
from webots_ros2_core.webots_node import WebotsNode
from std_msgs.msg import Bool

class SimpleNode(WebotsNode):
    def __init__(self, args):
        super().__init__(name='simple_node', args= args)
        self.node = rclpy.create_node('simple_node')
        self.root = self.robot.getRoot()   
        self.node.create_subscription(Bool, 'topic', self.simple_callback, 1)

    def simple_callback(self, state):
        if state.data == True:
            self.get_logger().info(f'I am true')
        else:
            self.get_logger().info(f'I am false')

def main(args=None):
    rclpy.init(args=args)
    my_webots_driver = SimpleNode(args=args)
    my_webots_driver.start_device_manager()
    rclpy.spin(my_webots_driver)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Steps to Reproduce

1. Run "ros2 launch webots_ros2_universal_robot moveit_demo_launch.py"
2. ros2 run the above node
3. Run a moveit action server/client, to command the robot to go to a new pose. I personally used this: https://github.com/AndrejOrsula/pymoveit2/tree/67f455fad35af0b69c4f754f1311175ac00cb7b0 since it's a Python wrapper.
4. Moveit client won't find the "joint_trajectory_controller/follow_joint_trajectory" action server

Expected behavior
Moveit client finds the "joint_trajectory_controller/follow_joint_trajectory" action server and executes the command

System

• Webots Version: R2022a
• ROS Version: Foxy
• Operating System: Ubuntu 20.04
• Graphics Card: NVIDIA RTX 3080