[unittest] Changed topic name + added checks

cmastalli · Jan 27, 2024 · f8046c5 · f8046c5
1 parent 6102545
commit f8046c5
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 4 deletions.
diff --git a/unittest/test_whole_body_state.py b/unittest/test_whole_body_state.py
@@ -82,8 +82,8 @@ def setUp(self):
 
     def test_publisher_without_contact(self):
         model = pinocchio.buildSampleModelHumanoid()
-        sub = WholeBodyStateRosSubscriber(model, "whole_body_state")
-        pub = WholeBodyStateRosPublisher(model, "whole_body_state")
+        sub = WholeBodyStateRosSubscriber(model, "whole_body_state_without_contact")
+        pub = WholeBodyStateRosPublisher(model, "whole_body_state_without_contact")
         time.sleep(1)
         # publish whole-body state messages
         q = pinocchio.randomConfiguration(model)
@@ -94,6 +94,12 @@ def test_publisher_without_contact(self):
             pub.publish(self.t, q, v, tau)
             if sub.has_new_msg():
                 break
+        # get whole-body state
+        _t, _q, _v, _tau, _, _, _, _ = sub.get_state()
+        self.assertEqual(self.t, _t, "Wrong time interval")
+        self.assertTrue(np.allclose(q, _q, atol=1e-9), "Wrong q")
+        self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v")
+        self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau")
 
     def test_communication(self):
         model = pinocchio.buildSampleModelHumanoid()

diff --git a/unittest/test_whole_body_trajectory.py b/unittest/test_whole_body_trajectory.py
@@ -94,8 +94,8 @@ def setUp(self) -> None:
 
     def test_publisher_without_contact(self):
         model = pinocchio.buildSampleModelHumanoid()
-        sub = WholeBodyTrajectoryRosSubscriber(model, "whole_body_trajectory")
-        pub = WholeBodyTrajectoryRosPublisher(model, "whole_body_trajectory")
+        sub = WholeBodyTrajectoryRosSubscriber(model, "whole_body_trajectory_without_contact")
+        pub = WholeBodyTrajectoryRosPublisher(model, "whole_body_trajectory_without_contact")
         time.sleep(1)
         # publish whole-body trajectory messages
         N = len(self.ts)
@@ -110,6 +110,14 @@ def test_publisher_without_contact(self):
             pub.publish(self.ts, xs, us)
             if sub.has_new_msg():
                 break
+        # get whole-body trajectory
+        _ts, _xs, _us, _, _, _, _ = sub.get_trajectory()
+        for i in range(N):
+            self.assertEqual(self.ts[i], _ts[i], "Wrong time interval at " + str(i))
+            self.assertTrue(
+                np.allclose(xs[i], _xs[i], atol=1e-9), "Wrong x at " + str(i)
+            )
+            self.assertTrue(np.allclose(us, _us, atol=1e-9), "Wrong u at " + str(i))
 
     def test_communication(self):
         model = pinocchio.buildSampleModelHumanoid()