detect_plug.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # stub for plug detection action
00003 
00004 import roslib; roslib.load_manifest('pr2_plugs_actions')
00005 import rospy;
00006 import actionlib;
00007 from pr2_plugs_msgs.msg import *
00008 from pr2_controllers_msgs.msg import *
00009 from actionlib_msgs.msg import *
00010 from trajectory_msgs.msg import JointTrajectoryPoint
00011 from joint_trajectory_action_tools.tools import *
00012 import PyKDL
00013 from tf_conversions.posemath import fromMsg, toMsg
00014 from math import pi
00015 
00016 
00017 def execute_cb(goal):
00018   rospy.loginfo("Action server received goal")
00019   preempt_timeout = rospy.Duration(5.0)
00020 
00021   for i in range(1,5):
00022     rospy.loginfo('Detecting plug in gripper from position %i'%i)
00023 
00024     # move to joint space position
00025     rospy.loginfo("Move in joint space...")
00026     if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/detect_plug_in_gripper%i'%i), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00027       rospy.logerr('Move retract in joint space failed')
00028       server.set_aborted()
00029       return
00030 
00031     # call vision plug detection
00032     rospy.loginfo("Detecting plug...")
00033     detect_plug_goal = VisionPlugDetectionGoal()
00034     detect_plug_goal.camera_name = "/r_forearm_cam"
00035     #detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0.0, -pi/9), PyKDL.Vector(-.03, 0, 0)).Inverse())
00036     detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(-pi/2, pi/9, 0.0), PyKDL.Vector(0.03, 0, -0.01)))
00037     detect_plug_goal.prior.header.stamp = rospy.Time.now()
00038     detect_plug_goal.prior.header.frame_id = "r_gripper_tool_frame"
00039     detect_plug_goal.origin_on_right = True
00040     detect_plug_goal.prior.header.stamp = rospy.Time.now()
00041     if detect_plug_client.send_goal_and_wait(detect_plug_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED:
00042       server.set_succeeded(DetectPlugInGripperResult(detect_plug_client.get_result().plug_pose))      
00043       rospy.loginfo("Action server goal finished")  
00044       return
00045 
00046   # Failure
00047   rospy.logerr("Failed to detect plug in gripper")      
00048   server.set_aborted(DetectPlugInGripperResult(detect_plug_goal.prior))      
00049 
00050 
00051 
00052 if __name__ == '__main__':
00053   #Initialize the node
00054   name = 'detect_plug'
00055   rospy.init_node(name)
00056 
00057   # create action clients we use
00058   joint_space_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_generator', JointTrajectoryAction)
00059   joint_space_client.wait_for_server()
00060   joint_space_goal = JointTrajectoryGoal()
00061 
00062   detect_plug_client = actionlib.SimpleActionClient('vision_plug_detection', VisionPlugDetectionAction)
00063   detect_plug_client.wait_for_server()
00064   detect_plug_goal = VisionPlugDetectionGoal()
00065   rospy.loginfo('Connected to action clients')
00066 
00067   # create action server
00068   server = actionlib.simple_action_server.SimpleActionServer(name, DetectPlugInGripperAction, execute_cb)
00069   rospy.loginfo('%s: Action server running', name)
00070 
00071 
00072   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