pr2_plugs_actions: recharge_toplevel.py Source File

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 import roslib
00035 roslib.load_manifest('pr2_plugs_actions')
00036 
00037 import rospy
00038 
00039 import threading
00040 import copy
00041 
00042 import actionlib
00043 import PyKDL
00044 
00045 from pr2_plugs_msgs.msg import *
00046 from pr2_plugs_msgs.srv import *
00047 from actionlib_msgs.msg import *
00048 from move_base_msgs.msg import *
00049 from geometry_msgs.msg import *
00050 from pr2_controllers_msgs.msg import *
00051 from pr2_common_action_msgs.msg import *
00052 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
00053 from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal
00054 
00055 from std_srvs.srv import *
00056 
00057 from tf_conversions.posemath import fromMsg, toMsg
00058 from pr2_arm_move_ik.tools import *
00059 
00060 # State machine classes
00061 import smach
00062 from smach import *
00063 from smach_ros import *
00064 
00065 from detect_outlet import construct_sm as construct_detect_outlet_sm
00066 from fetch_plug import construct_sm as construct_fetch_plug_sm
00067 from plug_in import construct_sm as construct_plug_in_sm
00068 
00069 from pr2_plugs_actions.tf_util import TFUtil
00070 
00071 def main():
00072     rospy.init_node("recharge_toplevel")
00073     TFUtil()
00074 
00075     # Close gripper goal
00076     close_gripper_goal = Pr2GripperCommandGoal()
00077     close_gripper_goal.command.position = 0.0
00078     close_gripper_goal.command.max_effort = 99999
00079 
00080     open_gripper_goal = Pr2GripperCommandGoal()
00081     open_gripper_goal.command.position = 0.07
00082     open_gripper_goal.command.max_effort = 99999
00083 
00084     # Clear/stow left arm
00085     clear_l_arm_goal = JointTrajectoryGoal()
00086     clear_l_arm_goal.trajectory.joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint',
00087                                                'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
00088     clear_l_arm_goal.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0],
00089                                                                    time_from_start=rospy.Duration(1.0)))
00090 
00091 
00092     stow_l_arm_goal = copy.deepcopy(clear_l_arm_goal)
00093     stow_l_arm_goal.trajectory.points = []
00094     stow_l_arm_goal.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.0,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0],
00095                                                                 time_from_start=rospy.Duration(1.0)))
00096     stow_l_arm_goal.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.06,  1.24,   1.65,  -1.68,  -1.73,  -0.1,  0.0],
00097                                                                    time_from_start=rospy.Duration(2.0)))
00098 
00099     
00100 
00101     # Construct state machine
00102     recharge_sm = StateMachine(
00103             outcomes=['plugged_in','unplugged','aborted','preempted'],
00104             input_keys = ['recharge_command'],
00105             output_keys = ['recharge_state'])
00106 
00107     # Set the initial state explicitly
00108     recharge_sm.set_initial_state(['PROCESS_RECHARGE_COMMAND'])
00109     recharge_sm.userdata.recharge_state = RechargeState(state=RechargeState.UNPLUGGED)
00110 
00111     recharge_sm.userdata.base_to_outlet        = Pose(position=Point(-0.0178, -0.7474,  0.2824), orientation=Quaternion( 0.0002, -0.0002, -0.7061, 0.7081))
00112     recharge_sm.userdata.gripper_to_plug_grasp = Pose(position=Point( 0.0282, -0.0050, -0.0103), orientation=Quaternion(-0.6964,  0.1228,  0.1228, 0.6964))
00113     recharge_sm.userdata.base_to_plug_on_base  = Pose(position=Point( 0.0783,  0.0244,  0.2426), orientation=Quaternion( 0.4986,  0.4962,  0.4963, 0.5088))
00114     recharge_sm.userdata.gripper_to_plug       = Pose(position=Point( 0.0275, -0.0046, -0.0094), orientation=Quaternion(-0.6876,  0.1293,  0.1226, 0.7039))
00115 
00116     with recharge_sm:
00117         ### PLUGGING IN ###
00118         @smach.cb_interface(input_keys=['recharge_command'])
00119         def plug_in_cond(ud):
00120             command = ud.recharge_command.command
00121             if command is RechargeCommand.PLUG_IN:
00122                 return True
00123             elif command is RechargeCommand.UNPLUG:
00124                 return False
00125         StateMachine.add('PROCESS_RECHARGE_COMMAND',
00126                 ConditionState(cond_cb = plug_in_cond),
00127                 { 'true':'NAVIGATE_TO_OUTLET',
00128                     'false':'UNPLUG'})
00129         
00130         sm_nav = StateMachine(
00131                 outcomes=['succeeded','aborted','preempted'],
00132                 input_keys = ['recharge_command'])
00133         StateMachine.add('NAVIGATE_TO_OUTLET', sm_nav,
00134                 {'succeeded':'DETECT_OUTLET',
00135                     'aborted':'FAIL_STILL_UNPLUGGED'})
00136         with sm_nav:
00137             StateMachine.add('GOAL_IS_LOCAL', 
00138                     ConditionState(
00139                         cond_cb = lambda ud: ud.recharge_command.plug_id == 'local',
00140                         input_keys = ['recharge_command']),
00141                     {'true': 'UNTUCK_AT_OUTLET',
00142                         'false': 'SAFETY_TUCK'})
00143             StateMachine.add('SAFETY_TUCK', 
00144                     SimpleActionState('tuck_arms', TuckArmsAction,
00145                         goal = TuckArmsGoal(True,True)),
00146                     { 'succeeded':'GET_OUTLET_LOCATIONS',
00147                       'aborted':'SAFETY_TUCK'})
00148             StateMachine.add('GET_OUTLET_LOCATIONS',
00149                     ServiceState('outlet_locations', GetOutlets,
00150                         response_slots=['poses']),
00151                     {'succeeded':'NAVIGATE'},
00152                     remapping={'poses':'approach_poses'})
00153 
00154             @smach.cb_interface(input_keys=['approach_poses','recharge_command'])
00155             def get_outlet_approach_goal(ud,goal):
00156                 """Get the approach pose from the outlet approach poses list"""
00157 
00158                 # Get id from command
00159                 plug_id = ud.recharge_command.plug_id
00160 
00161                 # Grab the relevant outlet approach pose
00162                 for outlet in ud.approach_poses:
00163                     if outlet.name == plug_id or outlet.id == plug_id:
00164                         target_pose = PoseStamped(pose=outlet.approach_pose)
00165 
00166                 # Create goal for move base
00167                 move_base_goal = MoveBaseGoal()
00168                 move_base_goal.target_pose = target_pose
00169                 move_base_goal.target_pose.header.stamp = rospy.Time.now()
00170                 move_base_goal.target_pose.header.frame_id = "map"
00171 
00172                 return move_base_goal
00173             StateMachine.add('NAVIGATE', 
00174                              SimpleActionState('pr2_move_base',MoveBaseAction,
00175                                                goal_cb=get_outlet_approach_goal,
00176                                                exec_timeout = rospy.Duration(20*60.0)),
00177                              { 'succeeded':'UNTUCK_AT_OUTLET' })
00178             StateMachine.add('UNTUCK_AT_OUTLET', 
00179                              SimpleActionState('tuck_arms', TuckArmsAction,
00180                                                goal = TuckArmsGoal(False, False)))
00181 
00182         StateMachine.add('DETECT_OUTLET', 
00183                          SimpleActionState('detect_outlet',DetectOutletAction,
00184                                            result_slots = ['base_to_outlet_pose']),
00185                          {'succeeded':'FETCH_PLUG',
00186                           'aborted':'FAIL_STILL_UNPLUGGED'},
00187                          remapping = {'base_to_outlet_pose':'base_to_outlet'})
00188 
00189         StateMachine.add('FETCH_PLUG',
00190                          SimpleActionState('fetch_plug',FetchPlugAction,
00191                                            result_slots = ['plug_on_base_pose', 'gripper_plug_grasp_pose']),
00192                          {'succeeded':'PLUG_IN',
00193                           'aborted':'FAIL_OPEN_GRIPPER'},
00194                          remapping = {'plug_on_base_pose':'base_to_plug_on_base', 'gripper_plug_grasp_pose':'gripper_to_plug_grasp'})
00195         
00196         @smach.cb_interface(input_keys=['recharge_state'], output_keys=['recharge_state'])
00197         def set_plug_in_result(ud, result_status, result):
00198             if result_status == GoalStatus.SUCCEEDED:
00199                 ud.recharge_state.state = RechargeState.PLUGGED_IN
00200         StateMachine.add('PLUG_IN',
00201                          SimpleActionState('plug_in',PlugInAction,
00202                                            goal_slots = ['base_to_outlet'],
00203                                            result_slots = ['gripper_to_plug'],
00204                                            result_cb = set_plug_in_result),
00205                          { 'succeeded':'STOW_LEFT_ARM',
00206                            'aborted':'RECOVER_STOW_PLUG'})
00207         
00208         # Move L arm out of the way
00209         StateMachine.add('STOW_LEFT_ARM',
00210                          SimpleActionState('l_arm_controller/joint_trajectory_action', JointTrajectoryAction,
00211                                            goal = stow_l_arm_goal),
00212                          { 'succeeded':'plugged_in'})
00213 
00214         ### UNPLUGGING ###
00215         unplug_sm = StateMachine(
00216             outcomes = ['succeeded','aborted','preempted'],
00217             input_keys=['recharge_state','gripper_to_plug_grasp','gripper_to_plug','base_to_outlet','base_to_plug_on_base'],
00218             output_keys=['recharge_state'])
00219         StateMachine.add('UNPLUG', unplug_sm,
00220                          { 'succeeded':'unplugged',
00221                            'aborted':'FAIL_OPEN_GRIPPER'})
00222         with unplug_sm:
00223             """Unplug from outlet"""
00224             # Make sure the gripper is held tightly
00225             StateMachine.add('CLOSE_GRIPPER',
00226                     SimpleActionState('r_gripper_controller/gripper_action', Pr2GripperCommandAction,
00227                         goal = close_gripper_goal),
00228                     { 'succeeded':'aborted',
00229                         'aborted':'CLEAR_LEFT_ARM'})
00230             
00231             # Move L arm out of the way
00232             StateMachine.add('CLEAR_LEFT_ARM',
00233                              SimpleActionState('l_arm_controller/joint_trajectory_action', JointTrajectoryAction,
00234                                                goal = clear_l_arm_goal),
00235                              { 'succeeded':'WIGGLE_OUT'})
00236 
00237             # Wiggle out
00238             def get_wiggle_out_goal(ud, goal):
00239                 # Set period
00240                 goal.wiggle_period = rospy.Duration(0.5)
00241                 goal.insert = 0
00242                 return goal
00243             StateMachine.add('WIGGLE_OUT',
00244                     SimpleActionState('wiggle_plug',WigglePlugAction,
00245                         goal_slots = ['gripper_to_plug','base_to_outlet'],
00246                         goal_cb = get_wiggle_out_goal),
00247                     {'succeeded':'PULL_BACK_FROM_WALL'})
00248 
00249             @smach.cb_interface(input_keys=['base_to_outlet','gripper_to_plug'])
00250             def get_pull_back_goal(ud, goal):
00251                 """Generate an ik goal to move along the local x axis of the outlet."""
00252 
00253                 pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(-0.10, 0, 0))
00254                 pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00255 
00256                 pose_base_wrist = fromMsg(ud.base_to_outlet) * pose_outlet_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist
00257 
00258                 goal.pose.pose = toMsg(pose_base_wrist)
00259                 goal.pose.header.stamp = rospy.Time.now()
00260                 goal.pose.header.frame_id = 'base_link'
00261                 goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
00262                 goal.move_duration = rospy.Duration(5.0)
00263 
00264             StateMachine.add('PULL_BACK_FROM_WALL',
00265                              SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_pull_back_goal),
00266                              {'succeeded':'STOW_PLUG',
00267                               'aborted':'WIGGLE_OUT'})
00268             
00269             # Stow plug
00270             @smach.cb_interface(input_keys=['recharge_state'],output_keys=['recharge_state'])
00271             def set_unplug_result(ud, result_state, result):
00272                 if result_state is GoalStatus.SUCCEEDED:
00273                     ud.recharge_state.state = RechargeState.UNPLUGGED
00274 
00275             StateMachine.add('STOW_PLUG', SimpleActionState('stow_plug',StowPlugAction,
00276                                                             goal_slots = ['gripper_to_plug_grasp','base_to_plug'],
00277                                                             result_cb = set_unplug_result),
00278                              {'succeeded':'SUCCEED_TUCK'},
00279                              remapping = {'base_to_plug':'base_to_plug_on_base'})
00280             
00281             StateMachine.add('SUCCEED_TUCK', SimpleActionState('tuck_arms', TuckArmsAction, goal=TuckArmsGoal(True, True)), { 'succeeded': 'SUCCEED_FREE_BASE' })
00282             
00283             # Shift base a little bit to free casters 
00284             @smach.cb_interface(input_keys=[])
00285             def get_free_base_goal(ud, goal):
00286                 goal.target_pose.pose.position.y =  -0.05
00287                 goal.target_pose.pose.position.x =  0.02
00288                 goal.target_pose.pose.orientation.w = 1.0
00289                 goal.target_pose.header.stamp = rospy.Time.now()
00290                 goal.target_pose.header.frame_id = "base_footprint"
00291 
00292             StateMachine.add('SUCCEED_FREE_BASE', SimpleActionState('move_base_omnidirectional', MoveBaseAction, goal_cb = get_free_base_goal), { 'succeeded': 'succeeded', 'aborted': 'succeeded' })
00293 
00294         ### RECOVERY STATES ###
00295         StateMachine.add('RECOVER_STOW_PLUG', SimpleActionState('stow_plug',StowPlugAction,
00296                                                                 goal_slots = ['gripper_to_plug_grasp','base_to_plug']),
00297                          { 'succeeded':'FAIL_STILL_UNPLUGGED',
00298                            'aborted':'FAIL_OPEN_GRIPPER'},
00299                          remapping = {'base_to_plug':'base_to_plug_on_base'})
00300 
00301         ### FAILURE STATES ###
00302         # State to fail to if we're still unplugged
00303         @smach.cb_interface(input_keys=['recharge_state'],output_keys=['recharge_state'],outcomes=['done'])
00304         def remain_unplugged(ud):
00305             ud.recharge_state.state = RechargeState.UNPLUGGED
00306             return 'done'
00307         StateMachine.add('FAIL_STILL_UNPLUGGED', 
00308                          CBState(cb = remain_unplugged),
00309                          {'done':'FAIL_LOWER_SPINE'})
00310         
00311         # Make sure we're not holding onto the plug
00312         StateMachine.add('FAIL_OPEN_GRIPPER',
00313                          SimpleActionState('r_gripper_controller/gripper_action', Pr2GripperCommandAction,
00314                                            goal = open_gripper_goal),
00315                          {'succeeded':'FAIL_UNTUCK'})
00316         
00317         StateMachine.add('FAIL_UNTUCK',
00318                          SimpleActionState('tuck_arms',TuckArmsAction,
00319                                            goal = TuckArmsGoal(False, False)),
00320                          {'succeeded':'FAIL_LOWER_SPINE'})
00321         
00322         # Lower the spine, tuck arms on cleanup
00323         StateMachine.add('FAIL_LOWER_SPINE',
00324                 SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction,
00325                                   goal = SingleJointPositionGoal(position=0.02)),
00326                 {'succeeded':'FAIL_TUCK_ARMS'})
00327 
00328         StateMachine.add('FAIL_TUCK_ARMS',
00329                          SimpleActionState('tuck_arms', TuckArmsAction,
00330                                            goal = TuckArmsGoal(True,True)),
00331                          { 'succeeded':'aborted'})
00332 
00333 
00334     # Run state machine introspection server
00335     intro_server = IntrospectionServer('recharge',recharge_sm,'/RECHARGE')
00336     intro_server.start()
00337 
00338     # Run state machine action server 
00339     sms = ActionServerWrapper(
00340             'recharge', RechargeAction, recharge_sm,
00341             succeeded_outcomes = ['plugged_in','unplugged'],
00342             aborted_outcomes = ['aborted'],
00343             preempted_outcomes = ['preempted'],
00344             goal_slots_map = {'command':'recharge_command'},
00345             result_slots_map = {'state':'recharge_state'})
00346     sms.run_server()
00347 
00348     rospy.spin()
00349 
00350     intro_server.stop()
00351 
00352 if __name__ == "__main__":
00353     main()
00354