From 78e4dcb8876e93395b3c4ce0dd52491f540d1c03 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 12 Aug 2023 19:12:28 +0900 Subject: [PATCH] [spot_basic_behaviors] udpate elevator behaviors --- .../src/spot_basic_behaviors/elevator_behavior.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index ee139b8d9c..059c68aac8 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/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