$search

recharge_toplevel.py

Go to the documentation of this file.

#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#    * Redistributions of source code must retain the above copyright
#        notice, this list of conditions and the following disclaimer.
#    * Redistributions in binary form must reproduce the above
#        copyright notice, this list of conditions and the following
#        disclaimer in the documentation and/or other materials provided
#        with the distribution.
#    * Neither the name of the Willow Garage nor the names of its
#        contributors may be used to endorse or promote products derived
#        from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import roslib
roslib.load_manifest('pr2_plugs_actions')

import rospy

import threading
import copy

import actionlib
import PyKDL

from pr2_plugs_msgs.msg import *
from pr2_plugs_msgs.srv import *
from actionlib_msgs.msg import *
from move_base_msgs.msg import *
from geometry_msgs.msg import *
from pr2_controllers_msgs.msg import *
from pr2_common_action_msgs.msg import *
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal

from std_srvs.srv import *

from tf_conversions.posemath import fromMsg, toMsg
from pr2_arm_move_ik.tools import *

# State machine classes
import smach
from smach import *
from smach_ros import *

from detect_outlet import construct_sm as construct_detect_outlet_sm
from fetch_plug import construct_sm as construct_fetch_plug_sm
from plug_in import construct_sm as construct_plug_in_sm

from pr2_plugs_actions.tf_util import TFUtil

def main():
    rospy.init_node("recharge_toplevel")
    TFUtil()

    # Close gripper goal
    close_gripper_goal = Pr2GripperCommandGoal()
    close_gripper_goal.command.position = 0.0
    close_gripper_goal.command.max_effort = 99999

    open_gripper_goal = Pr2GripperCommandGoal()
    open_gripper_goal.command.position = 0.07
    open_gripper_goal.command.max_effort = 99999

    # Clear/stow left arm
    clear_l_arm_goal = JointTrajectoryGoal()
    clear_l_arm_goal.trajectory.joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint',
                                               'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
    clear_l_arm_goal.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0],
                                                                   time_from_start=rospy.Duration(1.0)))


    stow_l_arm_goal = copy.deepcopy(clear_l_arm_goal)
    stow_l_arm_goal.trajectory.points = []
    stow_l_arm_goal.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.0,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0],
                                                                time_from_start=rospy.Duration(1.0)))
    stow_l_arm_goal.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.06,  1.24,   1.65,  -1.68,  -1.73,  -0.1,  0.0],
                                                                   time_from_start=rospy.Duration(2.0)))

    

    # Construct state machine
    recharge_sm = StateMachine(
            outcomes=['plugged_in','unplugged','aborted','preempted'],
            input_keys = ['recharge_command'],
            output_keys = ['recharge_state'])

    # Set the initial state explicitly
    recharge_sm.set_initial_state(['PROCESS_RECHARGE_COMMAND'])
    recharge_sm.userdata.recharge_state = RechargeState(state=RechargeState.UNPLUGGED)

    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))
    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))
    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))
    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))

    with recharge_sm:
        ### PLUGGING IN ###
        @smach.cb_interface(input_keys=['recharge_command'])
        def plug_in_cond(ud):
            command = ud.recharge_command.command
            if command is RechargeCommand.PLUG_IN:
                return True
            elif command is RechargeCommand.UNPLUG:
                return False
        StateMachine.add('PROCESS_RECHARGE_COMMAND',
                ConditionState(cond_cb = plug_in_cond),
                { 'true':'NAVIGATE_TO_OUTLET',
                    'false':'UNPLUG'})
        
        sm_nav = StateMachine(
                outcomes=['succeeded','aborted','preempted'],
                input_keys = ['recharge_command'])
        StateMachine.add('NAVIGATE_TO_OUTLET', sm_nav,
                {'succeeded':'DETECT_OUTLET',
                    'aborted':'FAIL_STILL_UNPLUGGED'})
        with sm_nav:
            StateMachine.add('GOAL_IS_LOCAL', 
                    ConditionState(
                        cond_cb = lambda ud: ud.recharge_command.plug_id == 'local',
                        input_keys = ['recharge_command']),
                    {'true': 'UNTUCK_AT_OUTLET',
                        'false': 'SAFETY_TUCK'})
            StateMachine.add('SAFETY_TUCK', 
                    SimpleActionState('tuck_arms', TuckArmsAction,
                        goal = TuckArmsGoal(True,True)),
                    { 'succeeded':'GET_OUTLET_LOCATIONS',
                      'aborted':'SAFETY_TUCK'})
            StateMachine.add('GET_OUTLET_LOCATIONS',
                    ServiceState('outlet_locations', GetOutlets,
                        response_slots=['poses']),
                    {'succeeded':'NAVIGATE'},
                    remapping={'poses':'approach_poses'})

            @smach.cb_interface(input_keys=['approach_poses','recharge_command'])
            def get_outlet_approach_goal(ud,goal):
                """Get the approach pose from the outlet approach poses list"""

                # Get id from command
                plug_id = ud.recharge_command.plug_id

                # Grab the relevant outlet approach pose
                for outlet in ud.approach_poses:
                    if outlet.name == plug_id or outlet.id == plug_id:
                        target_pose = PoseStamped(pose=outlet.approach_pose)

                # Create goal for move base
                move_base_goal = MoveBaseGoal()
                move_base_goal.target_pose = target_pose
                move_base_goal.target_pose.header.stamp = rospy.Time.now()
                move_base_goal.target_pose.header.frame_id = "map"

                return move_base_goal
            StateMachine.add('NAVIGATE', 
                             SimpleActionState('pr2_move_base',MoveBaseAction,
                                               goal_cb=get_outlet_approach_goal,
                                               exec_timeout = rospy.Duration(20*60.0)),
                             { 'succeeded':'UNTUCK_AT_OUTLET' })
            StateMachine.add('UNTUCK_AT_OUTLET', 
                             SimpleActionState('tuck_arms', TuckArmsAction,
                                               goal = TuckArmsGoal(False, False)))

        StateMachine.add('DETECT_OUTLET', 
                         SimpleActionState('detect_outlet',DetectOutletAction,
                                           result_slots = ['base_to_outlet_pose']),
                         {'succeeded':'FETCH_PLUG',
                          'aborted':'FAIL_STILL_UNPLUGGED'},
                         remapping = {'base_to_outlet_pose':'base_to_outlet'})

        StateMachine.add('FETCH_PLUG',
                         SimpleActionState('fetch_plug',FetchPlugAction,
                                           result_slots = ['plug_on_base_pose', 'gripper_plug_grasp_pose']),
                         {'succeeded':'PLUG_IN',
                          'aborted':'FAIL_OPEN_GRIPPER'},
                         remapping = {'plug_on_base_pose':'base_to_plug_on_base', 'gripper_plug_grasp_pose':'gripper_to_plug_grasp'})
        
        @smach.cb_interface(input_keys=['recharge_state'], output_keys=['recharge_state'])
        def set_plug_in_result(ud, result_status, result):
            if result_status == GoalStatus.SUCCEEDED:
                ud.recharge_state.state = RechargeState.PLUGGED_IN
        StateMachine.add('PLUG_IN',
                         SimpleActionState('plug_in',PlugInAction,
                                           goal_slots = ['base_to_outlet'],
                                           result_slots = ['gripper_to_plug'],
                                           result_cb = set_plug_in_result),
                         { 'succeeded':'STOW_LEFT_ARM',
                           'aborted':'RECOVER_STOW_PLUG'})
        
        # Move L arm out of the way
        StateMachine.add('STOW_LEFT_ARM',
                         SimpleActionState('l_arm_controller/joint_trajectory_action', JointTrajectoryAction,
                                           goal = stow_l_arm_goal),
                         { 'succeeded':'plugged_in'})

        ### UNPLUGGING ###
        unplug_sm = StateMachine(
            outcomes = ['succeeded','aborted','preempted'],
            input_keys=['recharge_state','gripper_to_plug_grasp','gripper_to_plug','base_to_outlet','base_to_plug_on_base'],
            output_keys=['recharge_state'])
        StateMachine.add('UNPLUG', unplug_sm,
                         { 'succeeded':'unplugged',
                           'aborted':'FAIL_OPEN_GRIPPER'})
        with unplug_sm:
            """Unplug from outlet"""
            # Make sure the gripper is held tightly
            StateMachine.add('CLOSE_GRIPPER',
                    SimpleActionState('r_gripper_controller/gripper_action', Pr2GripperCommandAction,
                        goal = close_gripper_goal),
                    { 'succeeded':'aborted',
                        'aborted':'CLEAR_LEFT_ARM'})
            
            # Move L arm out of the way
            StateMachine.add('CLEAR_LEFT_ARM',
                             SimpleActionState('l_arm_controller/joint_trajectory_action', JointTrajectoryAction,
                                               goal = clear_l_arm_goal),
                             { 'succeeded':'WIGGLE_OUT'})

            # Wiggle out
            def get_wiggle_out_goal(ud, goal):
                # Set period
                goal.wiggle_period = rospy.Duration(0.5)
                goal.insert = 0
                return goal
            StateMachine.add('WIGGLE_OUT',
                    SimpleActionState('wiggle_plug',WigglePlugAction,
                        goal_slots = ['gripper_to_plug','base_to_outlet'],
                        goal_cb = get_wiggle_out_goal),
                    {'succeeded':'PULL_BACK_FROM_WALL'})

            @smach.cb_interface(input_keys=['base_to_outlet','gripper_to_plug'])
            def get_pull_back_goal(ud, goal):
                """Generate an ik goal to move along the local x axis of the outlet."""

                pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(-0.10, 0, 0))
                pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)

                pose_base_wrist = fromMsg(ud.base_to_outlet) * pose_outlet_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist

                goal.pose.pose = toMsg(pose_base_wrist)
                goal.pose.header.stamp = rospy.Time.now()
                goal.pose.header.frame_id = 'base_link'
                goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
                goal.move_duration = rospy.Duration(5.0)

            StateMachine.add('PULL_BACK_FROM_WALL',
                             SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_pull_back_goal),
                             {'succeeded':'STOW_PLUG',
                              'aborted':'WIGGLE_OUT'})
            
            # Stow plug
            @smach.cb_interface(input_keys=['recharge_state'],output_keys=['recharge_state'])
            def set_unplug_result(ud, result_state, result):
                if result_state is GoalStatus.SUCCEEDED:
                    ud.recharge_state.state = RechargeState.UNPLUGGED

            StateMachine.add('STOW_PLUG', SimpleActionState('stow_plug',StowPlugAction,
                                                            goal_slots = ['gripper_to_plug_grasp','base_to_plug'],
                                                            result_cb = set_unplug_result),
                             {'succeeded':'SUCCEED_TUCK'},
                             remapping = {'base_to_plug':'base_to_plug_on_base'})
            
            StateMachine.add('SUCCEED_TUCK', SimpleActionState('tuck_arms', TuckArmsAction, goal=TuckArmsGoal(True, True)), { 'succeeded': 'SUCCEED_FREE_BASE' })
            
            # Shift base a little bit to free casters 
            @smach.cb_interface(input_keys=[])
            def get_free_base_goal(ud, goal):
                goal.target_pose.pose.position.y =  -0.05
                goal.target_pose.pose.position.x =  0.02
                goal.target_pose.pose.orientation.w = 1.0
                goal.target_pose.header.stamp = rospy.Time.now()
                goal.target_pose.header.frame_id = "base_footprint"

            StateMachine.add('SUCCEED_FREE_BASE', SimpleActionState('move_base_omnidirectional', MoveBaseAction, goal_cb = get_free_base_goal), { 'succeeded': 'succeeded', 'aborted': 'succeeded' })

        ### RECOVERY STATES ###
        StateMachine.add('RECOVER_STOW_PLUG', SimpleActionState('stow_plug',StowPlugAction,
                                                                goal_slots = ['gripper_to_plug_grasp','base_to_plug']),
                         { 'succeeded':'FAIL_STILL_UNPLUGGED',
                           'aborted':'FAIL_OPEN_GRIPPER'},
                         remapping = {'base_to_plug':'base_to_plug_on_base'})

        ### FAILURE STATES ###
        # State to fail to if we're still unplugged
        @smach.cb_interface(input_keys=['recharge_state'],output_keys=['recharge_state'],outcomes=['done'])
        def remain_unplugged(ud):
            ud.recharge_state.state = RechargeState.UNPLUGGED
            return 'done'
        StateMachine.add('FAIL_STILL_UNPLUGGED', 
                         CBState(cb = remain_unplugged),
                         {'done':'FAIL_LOWER_SPINE'})
        
        # Make sure we're not holding onto the plug
        StateMachine.add('FAIL_OPEN_GRIPPER',
                         SimpleActionState('r_gripper_controller/gripper_action', Pr2GripperCommandAction,
                                           goal = open_gripper_goal),
                         {'succeeded':'FAIL_UNTUCK'})
        
        StateMachine.add('FAIL_UNTUCK',
                         SimpleActionState('tuck_arms',TuckArmsAction,
                                           goal = TuckArmsGoal(False, False)),
                         {'succeeded':'FAIL_LOWER_SPINE'})
        
        # Lower the spine, tuck arms on cleanup
        StateMachine.add('FAIL_LOWER_SPINE',
                SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction,
                                  goal = SingleJointPositionGoal(position=0.02)),
                {'succeeded':'FAIL_TUCK_ARMS'})

        StateMachine.add('FAIL_TUCK_ARMS',
                         SimpleActionState('tuck_arms', TuckArmsAction,
                                           goal = TuckArmsGoal(True,True)),
                         { 'succeeded':'aborted'})


    # Run state machine introspection server
    intro_server = IntrospectionServer('recharge',recharge_sm,'/RECHARGE')
    intro_server.start()

    # Run state machine action server 
    sms = ActionServerWrapper(
            'recharge', RechargeAction, recharge_sm,
            succeeded_outcomes = ['plugged_in','unplugged'],
            aborted_outcomes = ['aborted'],
            preempted_outcomes = ['preempted'],
            goal_slots_map = {'command':'recharge_command'},
            result_slots_map = {'state':'recharge_state'})
    sms.run_server()

    rospy.spin()

    intro_server.stop()

if __name__ == "__main__":
    main()

---

pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Fri Mar 1 17:51:05 2013