door_sequence_service.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('door_executive')

import sys

import rospy
from std_srvs.srv import Empty, EmptyResponse
from door_manipulation_tools.srv import RobotPosture
from checkerboard_detector2.srv import Set, SetRequest

def service_call_sequence(req = None):
      service_names = []
      service_names.append('handle_detection_on')
      service_names.append('handle_detection_off')
      service_names.append('search_and_track_handle_on')
      service_names.append('search_and_track_handle_off')
      service_names.append('robot_pose')
      service_names.append('unlatch_door')
      resp = EmptyResponse()
      for service_name in service_names:
        rospy.loginfo('Waiting for ' + service_name)
        rospy.wait_for_service(service_name)

      try:
          start_handle_detection = rospy.ServiceProxy('handle_detection_on', Empty)
          rospy.loginfo('calling handle_detection_on')
          start_handle_detection()

          start_search_and_track = rospy.ServiceProxy('search_and_track_handle_on', Empty)
          rospy.loginfo('calling search_and_track_handle_on')
          start_search_and_track()

          robot_pose_client= rospy.ServiceProxy('robot_pose', RobotPosture)
          rospy.loginfo('calling robot_pose')
          robot_pose_client(['left tuck'])

          unlatch_client = rospy.ServiceProxy('unlatch_door', Empty)
          rospy.loginfo('calling unlatch_door')
          resp = unlatch_client ()

          rospy.loginfo('calling robot_pose')
          robot_pose_client(['left tuck'])
          #robot_pose_client(['left up', 'right up'])
          #robot_pose_client(['left tuck', 'right tuck'])

      except rospy.ServiceException, e:
          print "Service call failed: %s"%e
      finally:
          stop_search_and_track= rospy.ServiceProxy('search_and_track_handle_off', Empty)
          rospy.loginfo('calling search_and_track_handle_off')
          stop_search_and_track()
            
          stop_handle_detection = rospy.ServiceProxy('handle_detection_off', Empty)
          rospy.loginfo('calling handle_detection_off')
          stop_handle_detection()

      return resp

if __name__ == "__main__":
  rospy.init_node('door_opening_script')
  s = rospy.Service('start_door_opening_sequence', Empty, service_call_sequence)
  rospy.loginfo( "Ready to start door opening sequence")
  rospy.spin()