door_sequence_service_checkerboard.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
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('set_detect_checkerboard')
      service_names.append('search_and_track_checkerboard_on')
      service_names.append('search_and_track_checkerboard_off')
      service_names.append('robot_pose')
      service_names.append('unlatch_door')
      resp = None
      for service_name in service_names:
        rospy.loginfo('Waiting for ' + service_name)
        rospy.wait_for_service(service_name)

      try:
          detect_checkerboard = rospy.ServiceProxy('set_detect_checkerboard', Set)
          rospy.loginfo('calling set_detect_checkerboard')
          setactive = SetRequest()
          setactive.active = True
          detect_checkerboard(setactive)

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

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

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

          #swinger_client = rospy.ServiceProxy('swing_door', Empty)
          #rospy.loginfo('calling swing_door')
          #resp = swinger_client ()

          rospy.loginfo('calling set_detect_checkerboard')
          setinactive = SetRequest()
          setinactive.active = False
          detect_checkerboard(setinactive)

          rospy.loginfo('calling robot_pose')
          resp = 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_checkerboard_off', Empty)
          rospy.loginfo('calling search_and_track_checkerboard_off')
          resp = stop_search_and_track()
            
      return resp

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