detect_plug_on_base.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('pr2_plugs_actions')
00004 import rospy;
00005 import actionlib;
00006 from pr2_common_action_msgs.msg import *
00007 from pr2_plugs_msgs.msg import *
00008 from pr2_controllers_msgs.msg import *
00009 from joint_trajectory_action_tools.tools import *
00010 from pr2_arm_move_ik.tools import *
00011 from actionlib_msgs.msg import *
00012 import geometry_msgs.msg
00013 from trajectory_msgs.msg import JointTrajectoryPoint
00014 from math import pi
00015 import math
00016 import PyKDL
00017 from tf_conversions.posemath import fromMsg, toMsg
00018 from pr2_plugs_actions.tf_util import TFUtil
00019 from pr2_image_snapshot_recorder.msg import ImageSnapshotAction, ImageSnapshotGoal
00020 
00021 
00022 def to_init_position():
00023   rospy.loginfo("Move in joint space...")
00024   if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/detect_plug_on_base0'), rospy.Duration(20.0), rospy.Duration(5.0)) != GoalStatus.SUCCEEDED:
00025     rospy.logerr('Move retract in joint space failed')
00026     server.set_aborted()
00027     return
00028 
00029 
00030 
00031 def execute_cb(goal):
00032   rospy.loginfo("Action server received goal")
00033   preempt_timeout = rospy.Duration(5.0)
00034 
00035   # move spine up
00036   rospy.loginfo("Moving up spine...")
00037   spine_goal.position = 0.16
00038   if spine_client.send_goal_and_wait(spine_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00039     rospy.logerr('Moving up spine failed')
00040     server.set_aborted()
00041     return
00042 
00043   # we have 5 different plug detection poses
00044   to_init_position()
00045   for i in range(1,5):
00046     rospy.loginfo('Detecting plug on base from position %i'%i)
00047 
00048     # move to joint space position
00049     rospy.loginfo("Move in joint space...")
00050     if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/detect_plug_on_base%i'%i), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00051       rospy.logerr('Move retract in joint space failed')
00052       server.set_aborted()
00053       return
00054 
00055     if goal.record_image:
00056       rospy.loginfo("Recording images of plug on base")
00057       image_client = actionlib.SimpleActionClient('image_snapshot', ImageSnapshotAction)
00058       if not image_client.wait_for_server(rospy.Duration(20.0)):
00059         rospy.logerr("Imagesnapshot server is down.")
00060       else:
00061         image_goal = ImageSnapshotGoal()
00062         image_goal.topic_name = 'r_forearm_cam/image_raw'
00063         image_goal.num_images = 5
00064         image_goal.output_file_name = '/removable/continuous_operation/stow_plug_failure_images_%s.bag'%str(rospy.Time.now())
00065         image_client.send_goal_and_wait(image_goal, rospy.Duration(20.0), preempt_timeout)
00066 
00067     # call vision plug detection
00068     rospy.loginfo("Detecting plug...")
00069     detect_plug_goal = VisionPlugDetectionGoal()
00070     detect_plug_goal.camera_name = "/r_forearm_cam"
00071     detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0, pi/2), PyKDL.Vector(0.080, 0.026, 0.23)))
00072     detect_plug_goal.prior.header.frame_id = "base_link"
00073     detect_plug_goal.origin_on_right = False
00074     detect_plug_goal.prior.header.stamp = rospy.Time.now()
00075     if detect_plug_client.send_goal_and_wait(detect_plug_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED:
00076       pose_detect_plug = detect_plug_client.get_result().plug_pose
00077       try:
00078         pose_base_plug = fromMsg(TFUtil.wait_and_transform('base_link', pose_detect_plug).pose)
00079       except rospy.ServiceException, e:
00080         rospy.logerr('Could not transform between base_link and %s' %pose_detect_plug.header.frame_id)
00081         server.set_aborted()
00082         return
00083       error = (pose_base_plug.Inverse() * fromMsg(detect_plug_goal.prior.pose))
00084       (r, p, y) = error.M.GetRPY()
00085       if (math.fabs(r) < 0.8) and (math.fabs(p) < 0.8) and (math.fabs(y) < 0.8) and (math.fabs(error.p[0]) < 0.05) and (math.fabs(error.p[1]) < 0.05) and (math.fabs(error.p[2]) < 0.05):
00086         to_init_position()
00087         server.set_succeeded(DetectPlugOnBaseResult(detect_plug_client.get_result().plug_pose))      
00088         rospy.loginfo("Action server goal finished")  
00089         return
00090       else:
00091         rospy.loginfo('Found the plug, but it is in a different location than I expected. Error: %f, %f, %f'%(math.fabs(error.p[0]), math.fabs(error.p[1]), math.fabs(error.p[2])))
00092 
00093   # Failure
00094   rospy.logerr("Failed to detect plug on base")      
00095   to_init_position()
00096   server.set_aborted()
00097 
00098 
00099 
00100 
00101 if __name__ == '__main__':
00102   #Initialize the node
00103   name = 'detect_plug_on_base'
00104   rospy.init_node(name)
00105 
00106   # transform listener
00107   TFUtil()
00108 
00109   joint_space_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_generator', JointTrajectoryAction)
00110   joint_space_client.wait_for_server()
00111   joint_space_goal = JointTrajectoryGoal()
00112 
00113   spine_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
00114   spine_client.wait_for_server()
00115   spine_goal = SingleJointPositionGoal()
00116 
00117   detect_plug_client = actionlib.SimpleActionClient('vision_plug_detection', VisionPlugDetectionAction)
00118   detect_plug_client.wait_for_server()
00119 
00120   rospy.loginfo('Connected to action clients')
00121 
00122   # create action server
00123   server = actionlib.simple_action_server.SimpleActionServer(name, DetectPlugOnBaseAction, execute_cb)
00124   rospy.loginfo('%s: Action server running', name)
00125 
00126 
00127   rospy.spin()


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13