00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import roslib
00035 roslib.load_manifest('pr2_plugs_executive')
00036
00037 import rospy
00038
00039 import os
00040 import sys
00041 import time
00042 import tf
00043
00044 import actionlib;
00045 from pr2_common_action_msgs.msg import *
00046 from pr2_plugs_msgs.msg import *
00047 from pr2_controllers_msgs.msg import *
00048 from actionlib_msgs.msg import *
00049 from trajectory_msgs.msg import JointTrajectoryPoint
00050 from pr2_plugs_actions.posestampedmath import PoseStampedMath
00051 from math import pi
00052
00053
00054
00055 actions = [('vision_outlet_detection',VisionOutletDetectionAction),]
00056
00057 def main():
00058 rospy.init_node("plugs_executive")
00059
00060
00061 transformer = tf.TransformListener(True, rospy.Duration(60.0))
00062 preempt_timeout = rospy.Duration(5.0)
00063
00064
00065 rospy.loginfo("Starting actions.")
00066 ac = dict()
00067 for (name,action) in actions:
00068 ac[name] = actionlib.SimpleActionClient(name,action)
00069
00070
00071 for (name,action) in actions:
00072 print "Wait for server "+name
00073 ac[name].wait_for_server()
00074
00075
00076 detect_outlet_goal = VisionOutletDetectionGoal()
00077
00078 rospy.loginfo("All actions started.")
00079 rospy.loginfo("Detecting outlet...")
00080 detect_outlet_goal.camera_name = "/forearm_camera_r"
00081 pose_base_outlet = PoseStampedMath()
00082 pose_base_outlet.fromEuler(-0.14, -0.82, 0.29, 0, 0, -pi/2)
00083 detect_outlet_goal.prior = pose_base_outlet.msg
00084 detect_outlet_goal.prior.header.stamp = rospy.Time.now()
00085 detect_outlet_goal.prior.header.frame_id = "base_link"
00086
00087 if ac['vision_outlet_detection'].send_goal_and_wait(detect_outlet_goal, rospy.Duration(30.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00088 rospy.logerr('Vision outlet detection failed')
00089 return
00090
00091
00092 outlet_pose = ac['vision_outlet_detection'].get_result().outlet_pose
00093
00094
00095 print "GOOD", outlet_pose
00096
00097 return
00098
00099
00100
00101 if __name__ == "__main__":
00102 main()
00103