move_to_handle.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib
roslib.load_manifest('door_manipulation_tools')
from std_srvs.srv import Empty
import rospy
import sys

from pr2_python import base
from geometry_msgs.msg import PoseStamped

def callback(data):
    rospy.loginfo(rospy.get_name()+"Going to the handle")
    thebase = base.Base()
    success = thebase.move_manipulable_pose(data.pose.position.x, 
                                            data.pose.position.y, 
                                            data.pose.position.z)
    if success:
      rospy.loginfo("Reached Door, Trying to manipulate")
      try:
          rospy.wait_for_service('/door_manipulation/unlatch_door')
          unlatch_service = rospy.ServiceProxy('/door_manipulation/unlatch_door', Empty)
          unlatch_service()

          rospy.wait_for_service('/door_manipulation/swing_door')
          swing_service = rospy.ServiceProxy('/door_manipulation/swing_door', Empty)
          swing_service()
      except rospy.ServiceException, e:
          print "Service call failed: %s"%e

    rospy.signal_shutdown("Assuming work is done. Bye")
    sys.exit()
    

def setup():
    rospy.Subscriber("/door_manipulation/handle", PoseStamped, callback)

    rospy.spin()

if __name__ == '__main__':
    rospy.init_node('move_base_to_handle')
    setup()