stow_plug.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #    * Redistributions of source code must retain the above copyright
00012 #        notice, this list of conditions and the following disclaimer.
00013 #    * Redistributions in binary form must reproduce the above
00014 #        copyright notice, this list of conditions and the following
00015 #        disclaimer in the documentation and/or other materials provided
00016 #        with the distribution.
00017 #    * Neither the name of the Willow Garage nor the names of its
00018 #        contributors may be used to endorse or promote products derived
00019 #        from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 
00035 import roslib; roslib.load_manifest('pr2_plugs_actions')
00036 import rospy;
00037 import PyKDL
00038 import actionlib;
00039 from pr2_common_action_msgs.msg import *
00040 from pr2_plugs_msgs.msg import *
00041 from pr2_controllers_msgs.msg import *
00042 from joint_trajectory_action_tools.tools import *
00043 from pr2_arm_move_ik.tools import *
00044 from tf_conversions.posemath import fromMsg, toMsg
00045 from actionlib_msgs.msg import *
00046 import geometry_msgs.msg
00047 from trajectory_msgs.msg import JointTrajectoryPoint
00048 from math import pi
00049 from pr2_plugs_actions.tf_util import TFUtil
00050 
00051 
00052 #server actionlib.simple_action_server.SimpleActionServer
00053 
00054 ERROR_TOL = 0.03 # Max error between current, desired plug position
00055 MAX_ITERS = 5    # 
00056 
00057 def execute_cb(goal):
00058   rospy.loginfo("Action server received goal")
00059   preempt_timeout = rospy.Duration(5.0)
00060 
00061   # move the spine up
00062   rospy.loginfo("Moving up spine...")
00063   spine_goal.position = 0.16
00064   if spine_client.send_goal_and_wait(spine_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00065     rospy.logerr('Moving up spine failed')
00066     server.set_aborted()
00067     return
00068 
00069 
00070   # return plug
00071   error = 1.0
00072   free_cord = False
00073   iter_idx = 0
00074   while error > ERROR_TOL and not rospy.is_shutdown() and iter_idx < MAX_ITERS:
00075     iter_idx += 1
00076 
00077     # appraoch in joint space when re-trying
00078     if free_cord:
00079       rospy.loginfo("Free plug cord by doing approach...")
00080       if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/free_plug_cord'), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00081         rospy.logerr('Free plug cord failed')
00082         server.set_aborted()
00083         return
00084     free_cord = True
00085 
00086     # move to joint space position
00087     rospy.loginfo("Move in joint space...")
00088     if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/stow_plug'), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00089       rospy.logerr('Move approach in joint space failed')
00090       server.set_aborted()
00091       return
00092 
00093     # return plug to original location
00094     cart_space_goal.ik_seed = get_action_seed('pr2_plugs_configuration/return_plug_seed')
00095     pose_plug_gripper = fromMsg(goal.gripper_to_plug_grasp).Inverse()  
00096     pose_base_plug = fromMsg(goal.base_to_plug)
00097     pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00098     cart_space_goal.pose.pose = toMsg(pose_base_plug * pose_plug_gripper * pose_gripper_wrist)
00099     cart_space_goal.pose.header.stamp = rospy.Time.now()
00100     cart_space_goal.pose.header.frame_id = "base_link"
00101     cart_space_goal.move_duration = rospy.Duration(3.0)
00102     if cart_space_client.send_goal_and_wait(cart_space_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00103       rospy.logerr('Failed to return plug to location where the plug was found when fetching it.')
00104       server.set_aborted()
00105       return
00106 
00107     # check if plug in correct location
00108     pose_base_gripper_measured = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame', rospy.Time.now()).pose)  
00109     pose_base_gripper_desired = pose_base_plug * pose_plug_gripper
00110     diff = pose_base_gripper_measured.Inverse() * pose_base_gripper_desired
00111     error = diff.p.Norm()
00112     (r, p, y) = diff.M.GetRPY()
00113     error += (r + p + y)/10.0
00114     rospy.loginfo("Diff between desired and actual plug storing poses = %f"%error)
00115 
00116   # open gripper
00117   rospy.loginfo("Open gripper...")  
00118   gripper_goal.command.position = 0.07
00119   gripper_goal.command.max_effort = 99999
00120   if gripper_client.send_goal_and_wait(gripper_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00121     rospy.logerr('Open gripper failed')
00122     server.set_aborted()
00123     return
00124 
00125   # retract arm
00126   rospy.loginfo("Releasing plug...")  
00127   pose_plug_approach = PyKDL.Frame(PyKDL.Vector(0, 0.05, 0))
00128   cart_space_goal.pose.pose = toMsg(pose_base_plug * pose_plug_approach * pose_plug_gripper * pose_gripper_wrist)
00129   cart_space_goal.pose.header.stamp = rospy.Time.now()
00130   cart_space_goal.pose.header.frame_id = "base_link"
00131   cart_space_goal.move_duration = rospy.Duration(3.0)
00132   if cart_space_client.send_goal_and_wait(cart_space_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00133     rospy.logerr('Failed to move arm away from where the plug got released')
00134     server.set_aborted()
00135     return
00136 
00137   # detect plug on base
00138   rospy.loginfo("Detect plug on base...")
00139   start_time = rospy.Time.now()
00140   while detect_plug_on_base_client.send_goal_and_wait(DetectPlugOnBaseGoal(), rospy.Duration(120.0), preempt_timeout) != GoalStatus.SUCCEEDED:
00141     if rospy.Time.now() > start_time + rospy.Duration(60):
00142       rospy.logerr("Can't detect plug on base. It is soo dark in here... Giving up, driving away, living on the edge!  But let's take an image to make sure.")
00143       detect_plug_on_base_client.send_goal_and_wait(DetectPlugOnBaseGoal(record_image=True), rospy.Duration(120.0), preempt_timeout)
00144       break
00145     rospy.logerr('Detecting plug on base failed, trying again...')
00146 
00147   # move the spine down
00148   rospy.loginfo("Moving down spine...")
00149   spine_goal.position = 0.01
00150   spine_client.send_goal(spine_goal)
00151 
00152   # return result
00153   server.set_succeeded(StowPlugResult())
00154   rospy.loginfo("Action server goal finished")  
00155 
00156 
00157 if __name__ == '__main__':
00158   #Initialize the node
00159   name = 'stow_plug'
00160   rospy.init_node(name)
00161 
00162   # transform listener
00163   TFUtil()
00164 
00165   # create action clients we use
00166   gripper_client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action', Pr2GripperCommandAction)
00167   gripper_client.wait_for_server()
00168   gripper_goal = Pr2GripperCommandGoal()
00169 
00170   joint_space_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_generator', JointTrajectoryAction)
00171   joint_space_client.wait_for_server()
00172   joint_space_goal = JointTrajectoryGoal()
00173 
00174   cart_space_client = actionlib.SimpleActionClient('r_arm_ik', ArmMoveIKAction)
00175   cart_space_client.wait_for_server()
00176   cart_space_goal = ArmMoveIKGoal()
00177 
00178   spine_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
00179   spine_client.wait_for_server()
00180   spine_goal = SingleJointPositionGoal()
00181 
00182   detect_plug_on_base_client = actionlib.SimpleActionClient('detect_plug_on_base', DetectPlugOnBaseAction)
00183   detect_plug_on_base_client.wait_for_server()
00184   rospy.loginfo('Connected to action clients')
00185 
00186   # create action server
00187   server = actionlib.simple_action_server.SimpleActionServer(name, StowPlugAction, execute_cb)
00188   rospy.loginfo('%s: Action server running', name)
00189 
00190 
00191   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