add test case for complete stop

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
CMU-cabot · Mar 22, 2024 · 35dde1d · 35dde1d
1 parent 06a1e24
commit 35dde1d
Showing 1 changed file with 91 additions and 0 deletions.
diff --git a/cabot_site_test_room/cabot_site_test_room/tests-exhibit.py b/cabot_site_test_room/cabot_site_test_room/tests-exhibit.py
@@ -10,6 +10,97 @@ def wait_ready(tester):
     tester.wait_localization_started()
 
 
+def test5_navigation_to_an_exhibit_complete_stop(tester):
+    tester.reset_position()
+    tester.setup_actors(actors=[
+        {
+            "name": 'actor0',
+            "module": "pedestrian.stand_with_noise",
+            "params": {
+                "init_x": 10.0,
+                "init_y": 11.0,
+                "init_a": -90.0,
+                "std_x": 0.01,
+                "std_y": 0.01,
+            },
+        },
+        {
+            "name": 'actor1',
+            "module": "pedestrian.stand_with_noise",
+            "params": {
+                "init_x": 10.0,
+                "init_y": 12.0,
+                "init_a": 0.0,
+                "std_x": 0.01,
+                "std_y": 0.01,
+            },
+        },
+    ])
+    tester.goto_node('EDITOR_node_1710807829757')
+    tester.wait_for(10)
+    cancel = tester.check_topic_error(
+        action="check_complete_stop",
+        topic="/cabot/cmd_vel",
+        topic_type="geometry_msgs/msg/Twist",
+        condition="msg.linear.x == 0 and msg.angular.z > 0"
+    )
+    tester.wait_for(5)
+    cancel()
+    tester.setup_actors(actors=[
+        {
+            "name": 'actor0',
+            "module": "pedestrian.pool",
+            "params": {
+                "init_x": 14.0,
+                "init_y": 11.0,
+                "init_a": -90.0
+            },
+        },
+        {
+            "name": 'actor1',
+            "module": "pedestrian.stand_with_noise",
+            "params": {
+                "init_x": 10.0,
+                "init_y": 12.0,
+                "init_a": 0.0,
+                "std_x": 0.01,
+                "std_y": 0.01,
+            },
+        },
+    ])
+    tester.wait_for(10)
+    cancel = tester.check_topic_error(
+        action="check_complete_stop",
+        topic="/cabot/cmd_vel",
+        topic_type="geometry_msgs/msg/Twist",
+        condition="msg.linear.x == 0 and msg.angular.z > 0"
+    )
+    tester.wait_for(5)
+    cancel()
+    tester.setup_actors(actors=[
+        {
+            "name": 'actor0',
+            "module": "pedestrian.pool",
+            "params": {
+                "init_x": 14.0,
+                "init_y": 11.0,
+                "init_a": -90.0,
+            },
+        },
+        {
+            "name": 'actor1',
+            "module": "pedestrian.pool",
+            "params": {
+                "init_x": 14.0,
+                "init_y": 12.0,
+                "init_a": 0.0,
+            },
+        },
+    ])
+
+    tester.wait_navigation_arrived(timeout=60)
+
+
 def test4_navigation_to_an_exhibit_and_then_elevator(tester):
     tester.reset_position()
     tester.goto_node('EDITOR_node_1707899235671')