Wait for controller being active

If we just wait for the action server we might send a goal too early.
UniversalRobots · Apr 9, 2024 · 762b824 · 762b824
1 parent dcb21ba
commit 762b824
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git 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
@@ -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",