[Tests] Add a ServerManager util

Otherwise, if a test fails, an exception is raised, and `process.kill()`
is never called.

src/CMakeLists.txt

@@ -82,6 +82,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
     rbprmfullbody.py
     rbprmstate.py
     state_alg.py
+    utils.py
     )
 
 SET(${PROJECT_NAME}_PYTHON_TOOLS

src/hpp/corbaserver/rbprm/utils.py

+# Copyright (c) 2020, CNRS
+# Authors: Guilhem Saurel <guilhem.saurel@laas.fr>
+
+import subprocess
+import time
+
+
+class ServerManager:
+    """A context to ensure a server is running."""
+    def __init__(self, server):
+        self.server = server
+        subprocess.run(['killall', self.server])
+
+    def __enter__(self):
+        self.process = subprocess.Popen(self.server)
+        # give it some time to start
+        time.sleep(3)
+
+    def __exit__(self, exc_type, exc_value, exc_traceback):
+        self.process.kill()

tests/python/test_rbprm_state_3d.py

 # Copyright (c) 2020, CNRS
 # Authors: Pierre Fernbach <pfernbac@laas.fr>
 import unittest
-import subprocess
-import time
-from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
+
 from hpp.corbaserver.rbprm.hyq import Robot
+from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
+
+from hpp.corbaserver.rbprm.utils import ServerManager
 
 
 class TestRBPRMstate3D(unittest.TestCase):
     def test_contacts_3d(self):
-        subprocess.run(["killall", "hpp-rbprm-server"])
-        process = subprocess.Popen("hpp-rbprm-server")
-        time.sleep(3)
-        fullbody = Robot()
-        fullbody.client.robot.setDimensionExtraConfigSpace(6)
-        fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
-        fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
-        fullbody.loadAllLimbs("static", nbSamples=10000)
-        q = fullbody.referenceConfig[::] + [0] * 6
-        fullbody.setCurrentConfig(q)
-        com = fullbody.getCenterOfMass()
-        contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId]
-        state = State(fullbody, q=q, limbsIncontact=contacts)
-        self.assertTrue(state.isBalanced())
-        self.assertTrue(state.isValid()[0])
-        self.assertTrue(state.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state.isLimbInContact(fullbody.rArmId))
-        self.assertTrue(state.isLimbInContact(fullbody.lArmId))
-        self.assertEqual(com, state.getCenterOfMass())
-
-        # remove rf contact :
-        state1, success = StateHelper.removeContact(state, fullbody.rLegId)
-        self.assertTrue(success)
-        self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state.isLimbInContact(fullbody.rArmId))
-        self.assertTrue(state.isLimbInContact(fullbody.lArmId))
-        self.assertEqual(com, state1.getCenterOfMass())
-
-        # create a new contact :
-        n = [0, 0, 1]
-        p = [0.45, -0.2, 0.002]
-        state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
-        self.assertTrue(success)
-        self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state.isLimbInContact(fullbody.rArmId))
-        self.assertTrue(state.isLimbInContact(fullbody.lArmId))
-        self.assertTrue(state2.isBalanced())
-        p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
-        self.assertEqual(n, n_real)
-
-        process.kill()
+        with ServerManager('hpp-rbprm-server'):
+            fullbody = Robot()
+            fullbody.client.robot.setDimensionExtraConfigSpace(6)
+            fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
+            fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
+            fullbody.loadAllLimbs("static", nbSamples=10000)
+            q = fullbody.referenceConfig[::] + [0] * 6
+            fullbody.setCurrentConfig(q)
+            com = fullbody.getCenterOfMass()
+            contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId]
+            state = State(fullbody, q=q, limbsIncontact=contacts)
+            self.assertTrue(state.isBalanced())
+            self.assertTrue(state.isValid()[0])
+            self.assertTrue(state.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state.isLimbInContact(fullbody.rArmId))
+            self.assertTrue(state.isLimbInContact(fullbody.lArmId))
+            self.assertEqual(com, state.getCenterOfMass())
+
+            # remove rf contact :
+            state1, success = StateHelper.removeContact(state, fullbody.rLegId)
+            self.assertTrue(success)
+            self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state.isLimbInContact(fullbody.rArmId))
+            self.assertTrue(state.isLimbInContact(fullbody.lArmId))
+            self.assertEqual(com, state1.getCenterOfMass())
+
+            # create a new contact :
+            n = [0, 0, 1]
+            p = [0.45, -0.2, 0.002]
+            state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
+            self.assertTrue(success)
+            self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state.isLimbInContact(fullbody.rArmId))
+            self.assertTrue(state.isLimbInContact(fullbody.lArmId))
+            self.assertTrue(state2.isBalanced())
+            p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
+            self.assertEqual(n, n_real)
 
 
 if __name__ == '__main__':

tests/python/test_rbprm_state_6d.py

 # Copyright (c) 2020, CNRS
 # Authors: Pierre Fernbach <pfernbac@laas.fr>
 import unittest
-import subprocess
-import time
+
 from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
 from talos_rbprm.talos import Robot
 
+from hpp.corbaserver.rbprm.utils import ServerManager
+
 
 class TestRBPRMstate6D(unittest.TestCase):
     def test_contacts_6d(self):
-        subprocess.run(["killall", "hpp-rbprm-server"])
-        process = subprocess.Popen("hpp-rbprm-server")
-        time.sleep(3)
-        fullbody = Robot()
-        fullbody.client.robot.setDimensionExtraConfigSpace(6)
-        fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
-        fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
-        fullbody.loadAllLimbs("static", nbSamples=10000)
-        q = fullbody.referenceConfig[::] + [0] * 6
-        fullbody.setCurrentConfig(q)
-        com = fullbody.getCenterOfMass()
-        contacts = [fullbody.rLegId, fullbody.lLegId]
-        state = State(fullbody, q=q, limbsIncontact=contacts)
-        self.assertTrue(state.isBalanced())
-        self.assertTrue(state.isValid()[0])
-        self.assertTrue(state.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state.isLimbInContact(fullbody.lLegId))
-        self.assertEqual(com, state.getCenterOfMass())
-
-        # remove rf contact :
-        state1, success = StateHelper.removeContact(state, fullbody.rLegId)
-        self.assertTrue(success)
-        self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
-        self.assertFalse(state1.isBalanced())
-        self.assertEqual(com, state1.getCenterOfMass())
+        with ServerManager('hpp-rbprm-server'):
+            fullbody = Robot()
+            fullbody.client.robot.setDimensionExtraConfigSpace(6)
+            fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
+            fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
+            fullbody.loadAllLimbs("static", nbSamples=10000)
+            q = fullbody.referenceConfig[::] + [0] * 6
+            fullbody.setCurrentConfig(q)
+            com = fullbody.getCenterOfMass()
+            contacts = [fullbody.rLegId, fullbody.lLegId]
+            state = State(fullbody, q=q, limbsIncontact=contacts)
+            self.assertTrue(state.isBalanced())
+            self.assertTrue(state.isValid()[0])
+            self.assertTrue(state.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state.isLimbInContact(fullbody.lLegId))
+            self.assertEqual(com, state.getCenterOfMass())
 
-        # create a new contact :
-        n = [0, 0, 1]
-        p = [0.15, -0.085, 0.002]
-        state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
-        self.assertTrue(success)
-        self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state2.isBalanced())
-        p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
-        self.assertEqual(n, n2)
-        for i in range(3):
-            self.assertAlmostEqual(p[i], p2[i], 2)
+            # remove rf contact :
+            state1, success = StateHelper.removeContact(state, fullbody.rLegId)
+            self.assertTrue(success)
+            self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
+            self.assertFalse(state1.isBalanced())
+            self.assertEqual(com, state1.getCenterOfMass())
 
-        # try to replace a contact :
-        p = [0.2, 0.085, 0.002]
-        state3, success = StateHelper.addNewContact(state2, fullbody.lLegId, p, n)
-        self.assertTrue(success)
-        self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state3.isBalanced())
+            # create a new contact :
+            n = [0, 0, 1]
+            p = [0.15, -0.085, 0.002]
+            state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
+            self.assertTrue(success)
+            self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state2.isBalanced())
+            p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
+            self.assertEqual(n, n2)
+            for i in range(3):
+                self.assertAlmostEqual(p[i], p2[i], 2)
 
-        ## try com projection:
-        com_target = com[::]
-        com_target[2] -= 0.08
-        success = state.projectToCOM(com_target)
-        self.assertTrue(success)
-        fullbody.setCurrentConfig(state.q())
-        com_state = fullbody.getCenterOfMass()
-        self.assertEqual(com_state, state.getCenterOfMass())
-        for i in range(3):
-            self.assertAlmostEqual(com_state[i], com_target[i], 4)
+            # try to replace a contact :
+            p = [0.2, 0.085, 0.002]
+            state3, success = StateHelper.addNewContact(state2, fullbody.lLegId, p, n)
+            self.assertTrue(success)
+            self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state3.isBalanced())
 
-        # try lockOtherJoints:
-        p = [0.1, -0.085, 0.002]
-        state4, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, lockOtherJoints=True)
-        self.assertTrue(success)
-        self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state4.isBalanced())
-        for i in range(7, 13):
-            self.assertAlmostEqual(state.q()[i], state4.q()[i])
-        for i in range(19, len(q)):
-            self.assertAlmostEqual(state.q()[i], state4.q()[i])
+            # try com projection:
+            com_target = com[::]
+            com_target[2] -= 0.08
+            success = state.projectToCOM(com_target)
+            self.assertTrue(success)
+            fullbody.setCurrentConfig(state.q())
+            com_state = fullbody.getCenterOfMass()
+            self.assertEqual(com_state, state.getCenterOfMass())
+            for i in range(3):
+                self.assertAlmostEqual(com_state[i], com_target[i], 4)
 
-        # try with a rotation
-        rot = [0.259, 0, 0, 0.966]
-        state5, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, rotation=rot)
-        self.assertTrue(success)
-        self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
-        self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
-        self.assertTrue(state5.isBalanced())
-        fullbody.setCurrentConfig(state5.q())
-        rf_pose = fullbody.getJointPosition(fullbody.rfoot)
-        for i in range(4):
-            self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
+            # try lockOtherJoints:
+            p = [0.1, -0.085, 0.002]
+            state4, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, lockOtherJoints=True)
+            self.assertTrue(success)
+            self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state4.isBalanced())
+            for i in range(7, 13):
+                self.assertAlmostEqual(state.q()[i], state4.q()[i])
+            for i in range(19, len(q)):
+                self.assertAlmostEqual(state.q()[i], state4.q()[i])
 
-        process.kill()
+            # try with a rotation
+            rot = [0.259, 0, 0, 0.966]
+            state5, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, rotation=rot)
+            self.assertTrue(success)
+            self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
+            self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
+            self.assertTrue(state5.isBalanced())
+            fullbody.setCurrentConfig(state5.q())
+            rf_pose = fullbody.getJointPosition(fullbody.rfoot)
+            for i in range(4):
+                self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
 
 
 if __name__ == '__main__':

tests/python/test_talos_walk_contacts.py

 # Copyright (c) 2020, CNRS
 # Authors: Pierre Fernbach <pfernbac@laas.fr>
-import subprocess
-from importlib import import_module
 import unittest
-import time
+from importlib import import_module
+
+from hpp.corbaserver.rbprm.utils import ServerManager
 
 PATH = "hpp.corbaserver.rbprm.scenarios.demos"
 
 
 class TestTalosWalkContact(unittest.TestCase):
     def test_talos_walk_contacts(self):
-        subprocess.run(["killall", "hpp-rbprm-server"])
-        process = subprocess.Popen("hpp-rbprm-server")
-        time.sleep(3)
-        module_scenario = import_module(PATH + ".talos_flatGround")
-        if not hasattr(module_scenario, 'ContactGenerator'):
-            self.assertTrue(False)
-        ContactGenerator = getattr(module_scenario, 'ContactGenerator')
-        cg = ContactGenerator()
-        cg.run()
-        self.assertTrue(len(cg.configs) > 5)
-        self.assertTrue(len(cg.configs) < 10)
-        self.assertEqual(cg.q_init, cg.configs[0])
-        self.assertEqual(cg.q_goal, cg.configs[-1])
-        process.kill()
+        with ServerManager('hpp-rbprm-server'):
+            module_scenario = import_module(PATH + ".talos_flatGround")
+            if not hasattr(module_scenario, 'ContactGenerator'):
+                self.assertTrue(False)
+            ContactGenerator = getattr(module_scenario, 'ContactGenerator')
+            cg = ContactGenerator()
+            cg.run()
+            self.assertTrue(len(cg.configs) > 5)
+            self.assertTrue(len(cg.configs) < 10)
+            self.assertEqual(cg.q_init, cg.configs[0])
+            self.assertEqual(cg.q_goal, cg.configs[-1])
 
 
 if __name__ == '__main__':

tests/python/test_talos_walk_path.py

 # Copyright (c) 2020, CNRS
 # Authors: Pierre Fernbach <pfernbac@laas.fr>
-import subprocess
-from importlib import import_module
 import os
 import unittest
-import time
+from importlib import import_module
+
+from hpp.corbaserver.rbprm.utils import ServerManager
 
 PATH = "hpp.corbaserver.rbprm.scenarios.demos"
 
 
 class TestTalosWalkPath(unittest.TestCase):
     def test_talos_walk_path(self):
-        subprocess.run(["killall", "hpp-rbprm-server"])
-        process = subprocess.Popen("hpp-rbprm-server")
-        time.sleep(3)
-        module_scenario = import_module(PATH + ".talos_flatGround_path")
-        if not hasattr(module_scenario, 'PathPlanner'):
-            self.assertTrue(False)
-        PathPlanner = getattr(module_scenario, 'PathPlanner')
-        planner = PathPlanner()
-        planner.run()
-        ps = planner.ps
-        self.assertEqual(ps.numberPaths(), 2)
-        self.assertEqual(ps.pathLength(0), ps.pathLength(1))
-        self.assertTrue(ps.pathLength(1) > 6.)
-        self.assertTrue(ps.pathLength(1) < 7.)
-        self.assertEqual(planner.q_init, ps.configAtParam(1, 0))
-        self.assertEqual(planner.q_goal, ps.configAtParam(1, ps.pathLength(1)))
-        process.kill()
+        with ServerManager('hpp-rbprm-server'):
+            module_scenario = import_module(PATH + ".talos_flatGround_path")
+            if not hasattr(module_scenario, 'PathPlanner'):
+                self.assertTrue(False)
+            PathPlanner = getattr(module_scenario, 'PathPlanner')
+            planner = PathPlanner()
+            planner.run()
+            ps = planner.ps
+            self.assertEqual(ps.numberPaths(), 2)
+            self.assertEqual(ps.pathLength(0), ps.pathLength(1))
+            self.assertTrue(ps.pathLength(1) > 6.)
+            self.assertTrue(ps.pathLength(1) < 7.)
+            self.assertEqual(planner.q_init, ps.configAtParam(1, 0))
+            self.assertEqual(planner.q_goal, ps.configAtParam(1, ps.pathLength(1)))
 
 
 if __name__ == '__main__':