[spot_basic_behaviors] udpate elevator behaviors

k-okada · Nov 22, 2023 · 78e4dcb · 78e4dcb
1 parent 47cf829
commit 78e4dcb
Showing 1 changed file with 12 additions and 2 deletions.
diff --git a/...bot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/...bot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py
@@ -73,11 +73,21 @@ def run_initial(self, start_node, end_node, edge, pre_edge):
             return False
 
         try:
-            rospy.wait_for_message('/spot_recognition/elevator_door_points', PointCloud2, timeout=rospy.Duration(30))
+            end_time = rospy.Time.now() + rospy.Duration(30)
+        except rospy.ROSException:
+            rospy.logerr("Door points topics are not published.")
+            return False
+
+        try:
             rospy.wait_for_message("/elevator_state_publisher/current_floor", Int16, timeout=rospy.Duration(30))
+        except rospy.ROSException:
+            rospy.logerr("Current floor topics are not published.")
+            return False
+
+        try:
             rospy.wait_for_message("/elevator_state_publisher/rest_elevator", Bool, timeout=rospy.Duration(30))
         except rospy.ROSException:
-            rospy.logerr("Some topics are not published.")
+            rospy.logerr("Elevator state topic are not published.")
             return False
 
         return True