From 762b8247c83c78e8616d976b99eccc5f8baeb0be Mon Sep 17 00:00:00 2001
From: Felix Exner <exner@fzi.de>
Date: Fri, 22 Dec 2023 07:59:39 +0000
Subject: [PATCH] Wait for controller being active

If we just wait for the action server we might send a goal too early.
---
 ur_simulation_gazebo/test/test_common.py | 2 ++
 ur_simulation_gazebo/test/test_gazebo.py | 5 ++---
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/ur_simulation_gazebo/test/test_common.py b/ur_simulation_gazebo/test/test_common.py
index 1902b2a..6b8b071 100644
--- a/ur_simulation_gazebo/test/test_common.py
+++ b/ur_simulation_gazebo/test/test_common.py
@@ -26,6 +26,7 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 import logging
+import time
 
 import rclpy
 from rclpy.action import ActionClient
@@ -77,6 +78,7 @@ def wait_for_controller(
                 controller_active = controller.state == "active"
                 logging.info("Controller '%s' is active.", controller_name)
                 return True
+        time.sleep(1)
     raise Exception(
         f"Could not find active controller '{controller_name}' within timeout of {timeout}"
     )
diff --git a/ur_simulation_gazebo/test/test_gazebo.py b/ur_simulation_gazebo/test/test_gazebo.py
index 172f839..db650b9 100644
--- a/ur_simulation_gazebo/test/test_gazebo.py
+++ b/ur_simulation_gazebo/test/test_gazebo.py
@@ -52,9 +52,7 @@
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 sys.path.append(os.path.dirname(__file__))
-from test_common import (  # noqa: E402
-    ActionInterface,
-)
+from test_common import ActionInterface, wait_for_controller  # noqa: E402
 
 
 TIMEOUT_EXECUTE_TRAJECTORY = 30
@@ -109,6 +107,7 @@ def tearDownClass(cls):
         rclpy.shutdown()
 
     def init_robot(self):
+        wait_for_controller(self.node, "joint_trajectory_controller", 30)
         self._follow_joint_trajectory = ActionInterface(
             self.node,
             "/joint_trajectory_controller/follow_joint_trajectory",