[spot_basicZ_behaviors] add pu/down to switchbot device name ntry

k-okada · Nov 22, 2023 · f4719ff · f4719ff
1 parent 6f80bd4
commit f4719ff
Showing 1 changed file with 5 additions and 3 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
@@ -96,6 +96,8 @@ def run_main(self, start_node, end_node, edge, pre_edge):
 
         rospy.logdebug('run_main() called')
 
+        start_floor = start_node.properties['floor']
+
         graph_name = edge.properties['graph']
         start_id = list(filter(
             lambda x: x['graph'] == graph_name,
@@ -145,7 +147,7 @@ def run_main(self, start_node, end_node, edge, pre_edge):
         rospy.loginfo('calling elevator when riding...')
         success_calling = False
         switchbot_goal = SwitchBotCommandGoal()
-        switchbot_goal.device_name = start_node.properties['switchbot_device']
+        switchbot_goal.device_name = start_node.properties['switchbot_device_up'] if end_floor > start_floor else start_node.properties['switchbot_device_down']
         switchbot_goal.command = 'press'
         count = 0
         while True:
@@ -186,7 +188,7 @@ def run_main(self, start_node, end_node, edge, pre_edge):
         # call elevator from destination floor while
         rospy.loginfo('calling elevator when getting off...')
         switchbot_goal = SwitchBotCommandGoal()
-        switchbot_goal.device_name = end_node.properties['switchbot_device']
+        switchbot_goal.device_name = end_node.properties['switchbot_device_up'] if end_floor > start_floor else end_node.properties['switchbot_device_down']
         switchbot_goal.command = 'press'
         self.action_client_switchbot.send_goal(switchbot_goal)
         ##
@@ -199,7 +201,7 @@ def run_main(self, start_node, end_node, edge, pre_edge):
         result_switchbot = self.action_client_switchbot.get_result()
         while not rospy.is_shutdown():
             switchbot_goal = SwitchBotCommandGoal()
-            switchbot_goal.device_name = start_node.properties['switchbot_device']
+            switchbot_goal.device_name = start_node.properties['switchbot_device_up'] if end_floor > start_floor else end_node.properties['switchbot_device_down']
             switchbot_goal.command = 'press'
             self.action_client_switchbot.send_goal_and_wait(switchbot_goal, execute_timeout=rospy.Duration(10))
             if self.spot_client.wait_for_navigate_to_result(rospy.Duration(5)):