iri_door_detector: Open_The_Door.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('iri_door_detector')
00004 import rospy
00005 import smach
00006 import smach_ros
00007 import tf
00008 import iri_door_detector.msg
00009 from geometry_msgs.msg import Point, Pose, Quaternion
00010 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00011 from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
00012 from trajectory_msgs.msg import JointTrajectoryPoint
00013 from arm_navigation_msgs.msg import *
00014 from smach_ros import SimpleActionState
00015 from smach import CBState
00016 from sensor_msgs.msg import JointState
00017 
00018 def main():
00019     rospy.init_node('smach_example_state_machine')
00020 
00021     sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
00022     with sm:
00023 
00024         #---smach callbacks
00025 
00026         def init_left_arm_cb(userdata, goal):
00027                 point = JointTrajectoryPoint()
00028                 point.time_from_start = rospy.Duration.from_sec(2)
00029                 point.positions = [ -0.5, 0.1, -0.1, 0.6109, 0.0, 0.0, 0.0 ]
00030                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00031                 joint_goal = JointTrajectoryGoal()
00032                 joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 
00033                                                      'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 
00034                                                      'arm_left_7_joint']
00035                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00036                 joint_goal.trajectory.points.append(point)
00037                 return joint_goal
00038 
00039         def init_right_arm_torso_cb(userdata,goal):
00040                 point = JointTrajectoryPoint()
00041                 point.time_from_start = rospy.Duration.from_sec(2)
00042                 point.positions = [ 0.0, 0.0, -0.5, 0.1, -0.1, 0.6109, 0.0, 0.0, 0.0 ]
00043                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00044                 joint_goal = JointTrajectoryGoal()
00045                 joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint',
00046                                                      'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint',
00047                                                      'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint']
00048                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00049                 joint_goal.trajectory.points.append(point)
00050                 return joint_goal
00051 
00052         def rise_left_arm_cb(userdata, goal):
00053                 point = JointTrajectoryPoint()
00054                 point.time_from_start = rospy.Duration.from_sec(2)
00055                 point.positions = [ 0.8, 0.1, -0.2, 1.8, -1.5, -1.0, 0.0 ]
00056                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00057                 joint_goal = JointTrajectoryGoal()
00058                 joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 
00059                                                      'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 
00060                                                      'arm_left_7_joint']
00061                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00062                 joint_goal.trajectory.points.append(point)
00063                 return joint_goal
00064 
00065         def rise_right_arm_torso_cb(userdata,goal):
00066                 point = JointTrajectoryPoint()
00067                 point.time_from_start = rospy.Duration.from_sec(2)
00068                 point.positions = [ 0.0, 0.0, 0.8, 0.1, -0.2, 1.8, -1.5, -1.0, 0.0 ]
00069                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00070                 joint_goal = JointTrajectoryGoal()
00071                 joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint',
00072                                                      'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint',
00073                                                      'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint']
00074                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00075                 joint_goal.trajectory.points.append(point)
00076                 return joint_goal
00077 
00078         
00079         def tuck_left_arm_cb(userdata, goal):
00080                 point = JointTrajectoryPoint()
00081                 point.time_from_start = rospy.Duration.from_sec(2)
00082                 point.positions = [ 0.3, 0.1, -1.0, 0.8, 0.0, 0.0, 0.0 ]
00083                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00084                 joint_goal = JointTrajectoryGoal()
00085                 joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 
00086                                                      'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 
00087                                                      'arm_left_7_joint']
00088                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00089                 joint_goal.trajectory.points.append(point)
00090                 return joint_goal
00091 
00092         def tuck_right_arm_torso_cb(userdata,goal):
00093                 point = JointTrajectoryPoint()
00094                 point.time_from_start = rospy.Duration.from_sec(2)
00095                 point.positions = [ 0.0, 0.0, 0.3, 0.1, -1.0, 0.8, 0.0, 0.0, 0.0 ]
00096                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00097                 joint_goal = JointTrajectoryGoal()
00098                 joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint',
00099                                                      'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint',
00100                                                      'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint']
00101                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00102                 joint_goal.trajectory.points.append(point)
00103                 return joint_goal
00104 
00105 
00106         def stretch_left_arm_cb(userdata, goal):
00107                 point = JointTrajectoryPoint()
00108                 point.time_from_start = rospy.Duration.from_sec(2)
00109                 point.positions = [ 1.3, 0.1, 0.0, 0.6, 0.0, 0.0, 0.0 ]
00110                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00111                 joint_goal = JointTrajectoryGoal()
00112                 joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 
00113                                                      'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 
00114                                                      'arm_left_7_joint']
00115                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00116                 joint_goal.trajectory.points.append(point)
00117                 return joint_goal
00118 
00119         def stretch_right_arm_torso_cb(userdata,goal):
00120                 point = JointTrajectoryPoint()
00121                 point.time_from_start = rospy.Duration.from_sec(2)
00122                 point.positions = [ 0.0, 0.0, 1.3, 0.1, 0.0, 0.6, 0.0, 0.0, 0.0 ]
00123                 point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
00124                 joint_goal = JointTrajectoryGoal()
00125                 joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint',
00126                                                      'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint',
00127                                                      'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint']
00128                 joint_goal.trajectory.header.stamp = rospy.get_rostime()
00129                 joint_goal.trajectory.points.append(point)
00130                 return joint_goal
00131 
00132         def find_a_door_goal_cb(userdata, goal):
00133                 find_a_door_goal = iri_door_detector.msg.FindADoorGoal()
00134                 find_a_door_goal.start = 1
00135                 return find_a_door_goal
00136 
00137         @smach.cb_interface(
00138                 outcomes=['succeeded_closed_left','succeeded_closed_right','succeeded_open_left','succeeded_open_right','succeeded_open','aborted','preempted'])
00139 
00140         def find_a_door_base_result_cb(userdata, status, result):
00141                 #according to actionlib_msgs/GoalStatus.msg, SUCCEEDED = 3
00142                 rospy.loginfo("status code %d" % status)
00143                 rospy.loginfo("\n door state: %s" % result.state + "\n base poses: \n %s" % result.base_poses)
00144                 if status == 3 and result.state == "fully_open": 
00145                         return 'succeeded_open'
00146                 elif status == 3 and result.state == "open_left": 
00147                         return 'succeeded_open_left'
00148                 elif status == 3 and result.state == "open_right": 
00149                         return 'succeeded_open_right'
00150                 elif status == 3 and result.state == "closed_left":
00151                         return 'succeeded_closed_left'
00152                 elif status == 3 and result.state == "closed_right": 
00153                         return 'succeeded_closed_right'
00154                 else:
00155                         return 'aborted'
00156 
00157         @smach.cb_interface(
00158                 outcomes=['succeeded_left', 'succeeded_right', 'aborted','preempted'])
00159 
00160         def find_a_door_arm_result_cb(userdata, status, result):
00161                 #according to actionlib_msgs/GoalStatus.msg, SUCCEEDED = 3
00162                 rospy.loginfo("status code %d" % status)
00163                 rospy.loginfo("\n door state: %s" % result.state + "\n arm poses: \n %s" %  result.arm_poses.goal_constraints.position_constraints[0].header + "\n %s" % result.arm_poses.goal_constraints.position_constraints[0].position + "\n %s" % result.arm_poses.goal_constraints.orientation_constraints[0].orientation )              
00164                 if (status == 3 and (result.state == "closed_right" or result.state == "open_right" or result.state == "fully_open")): 
00165                         return 'succeeded_right'
00166                 if (status == 3 and (result.state == "closed_left" or result.state == "open_left")): 
00167                         return 'succeeded_left'
00168                 if (status != 3):
00169                         return 'aborted'
00170 
00171         def turn_handle_goal_cb(userdata, goal):
00172                 goal.planner_service_name=userdata.planner_service_name
00173                 goal.motion_plan_request=userdata.motion_plan_request
00174                 goal.motion_plan_request.goal_constraints.position_constraints[0].position.z = userdata.motion_plan_request.goal_constraints.position_constraints[0].position.z -0.12
00175                 #goal.motion_plan_request.goal_constraints.position_constraints[0].position.y = userdata.motion_plan_request.goal_constraints.position_constraints[0].position.y + 0.05
00176                 return goal
00177 
00178         @smach.cb_interface(input_keys=['position'],
00179                 outcomes=['succeeded', 'aborted', 'preempted'])
00180 
00181         def joint_state_cb(userdata):
00182 
00183                 # Local cb
00184                 def joint_states_cb(joint_states, userdata):
00185                         #userdata.position = dict(zip(joint_states.name, joint_states.position))['arm_left_1_joint']
00186                         if joint_states.name:
00187                                 joint_sub.unregister()
00188  
00189                 # Subscribe to joint state messages
00190                 joint_sub = rospy.Subscriber("/joint_states", JointState, joint_states_cb, userdata)                    
00191 
00192                 position = joint_states.name
00193                 rospy.loginfo("Joints are %s" % position)
00194                                 
00195                 return 'succeeded'
00196                         
00197 
00198         def push_door_left_goal_cb(userdata, goal):
00199                 nav_goal = MoveBaseGoal()
00200                 nav_goal.target_pose.header.stamp    = rospy.Time.now()
00201                 nav_goal.target_pose.header.frame_id = "/base_link"
00202                 p = Point(0.7,-0.35,0.0)
00203                 q = tf.transformations.quaternion_from_euler(0, 0, -0.5)
00204                 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00205                 rospy.loginfo("\n push door goal: \n %s" % nav_goal.target_pose.pose)
00206                 return nav_goal
00207 
00208         def push_door_right_goal_cb(userdata, goal):
00209                 nav_goal = MoveBaseGoal()
00210                 nav_goal.target_pose.header.stamp    = rospy.Time.now()
00211                 nav_goal.target_pose.header.frame_id = "/base_link"
00212                 p = Point(0.7,0.35,0.0)
00213                 q = tf.transformations.quaternion_from_euler(0, 0, 0.5)
00214                 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00215                 rospy.loginfo("\n push door goal: \n %s" % nav_goal.target_pose.pose)
00216                 return nav_goal
00217 
00218         def finish_push_door_right_goal_cb(userdata, goal):
00219                 nav_goal = MoveBaseGoal()
00220                 nav_goal.target_pose.header.stamp    = rospy.Time.now()
00221                 nav_goal.target_pose.header.frame_id = "/base_link"
00222                 p = Point(1.2,-0.2,0.0)
00223                 q = tf.transformations.quaternion_from_euler(0, 0, 0)
00224                 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00225                 return nav_goal
00226 
00227         def finish_push_door_left_goal_cb(userdata, goal):
00228                 nav_goal = MoveBaseGoal()
00229                 nav_goal.target_pose.header.stamp    = rospy.Time.now()
00230                 nav_goal.target_pose.header.frame_id = "/base_link"
00231                 p = Point(1.2,0.2,0.0)
00232                 q = tf.transformations.quaternion_from_euler(0, 0, 0)
00233                 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00234                 return nav_goal
00235 
00236 
00237         #---smach states
00238 
00239         #Tuck arm
00240         smach.StateMachine.add('SAFE_POSITION_LEFT_ARM', 
00241                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00242                                 JointTrajectoryAction,
00243                                 goal_cb = tuck_left_arm_cb),
00244                                 transitions={'succeeded':'SAFE_POSITION_RIGHT_ARM', 
00245                                              'aborted':'SAFE_POSITION_LEFT_ARM'})
00246         #Tuck arm
00247         smach.StateMachine.add('SAFE_POSITION_RIGHT_ARM', 
00248                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00249                                 JointTrajectoryAction,
00250                                 goal_cb = tuck_right_arm_torso_cb),
00251                                 transitions={'succeeded':'FIND_A_DOOR_BASE', 
00252                                              'aborted':'SAFE_POSITION_LEFT_ARM'})
00253 
00254         #Find a door 
00255         smach.StateMachine.add('FIND_A_DOOR_BASE',
00256                 SimpleActionState('/iri_door_detector/door_detector_actions/find_a_door',
00257                                 iri_door_detector.msg.FindADoorAction,
00258                                 goal_cb=find_a_door_goal_cb,
00259                                 result_slots=['base_poses',
00260                                               'arm_poses',
00261                                               'planner_service_name',
00262                                               'state']),
00263                 transitions={'succeeded':'BASE_POSES',
00264                              'aborted':'SAFE_POSITION_LEFT_ARM'},
00265                 remapping={'base_poses':'user_data_base_poses',
00266                            'arm_poses':'user_data_arm_poses',
00267                            'planner_service_name':'user_data_planner_service_name',
00268                            'state':'user_data_state'})
00269         
00270         #Print goal for move_base
00271         smach.StateMachine.add('BASE_POSES',
00272                 SimpleActionState('/iri_door_detector/door_detector_actions/find_a_door',
00273                                 iri_door_detector.msg.FindADoorAction,
00274                                 result_cb=find_a_door_base_result_cb),
00275                 transitions={'succeeded_open':'MOVE_BASE_OPEN',
00276                              'succeeded_open_left':'LIFT_ARM_LEFT',
00277                              'succeeded_open_right':'LIFT_ARM_RIGHT',
00278                              'succeeded_closed_left':'RISE_ARM_LEFT',
00279                              'succeeded_closed_right':'RISE_ARM_RIGHT',
00280                              'aborted':'FIND_A_DOOR_BASE'})
00281 
00282         #Prepare left arm for door opening
00283         smach.StateMachine.add('RISE_ARM_LEFT', 
00284                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00285                                 JointTrajectoryAction,
00286                                 goal_cb = rise_left_arm_cb),
00287                                 transitions={'succeeded':'MOVE_BASE_CLOSED', 
00288                                              'aborted':'SAFE_POSITION_LEFT_ARM'})
00289         #Prepare right arm for door opening
00290         smach.StateMachine.add('RISE_ARM_RIGHT', 
00291                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00292                                 JointTrajectoryAction,
00293                                 goal_cb = rise_right_arm_torso_cb),
00294                                 transitions={'succeeded':'MOVE_BASE_CLOSED', 
00295                                              'aborted':'SAFE_POSITION_LEFT_ARM'})
00296 
00297         #Move towards the door
00298         smach.StateMachine.add('MOVE_BASE_OPEN',
00299                 SimpleActionState('move_base',
00300                                 MoveBaseAction,
00301                                 goal_slots=['target_pose']),
00302                 transitions={'succeeded':'succeeded',
00303                              'aborted':'FIND_A_DOOR_BASE'},
00304                 remapping={'target_pose':'user_data_base_poses'})
00305 
00306         #Move towards the door
00307         smach.StateMachine.add('MOVE_BASE_CLOSED',
00308                 SimpleActionState('move_base',
00309                                 MoveBaseAction,
00310                                 goal_slots=['target_pose']),
00311                 transitions={'succeeded':'FIND_A_DOOR_ARM',
00312                              'aborted':'FIND_A_DOOR_BASE'},
00313                 remapping={'target_pose':'user_data_base_poses'})
00314 
00315         #Find a door 
00316         smach.StateMachine.add('FIND_A_DOOR_ARM',
00317                 SimpleActionState('/iri_door_detector/door_detector_actions/find_a_door',
00318                                 iri_door_detector.msg.FindADoorAction,
00319                                 goal_cb=find_a_door_goal_cb,
00320                                 result_slots=['arm_poses',
00321                                               'planner_service_name',
00322                                               'state']),
00323                 transitions={'succeeded':'ARM_POSES',
00324                              'aborted':'SAFE_POSITION_LEFT_ARM'},
00325                 remapping={'arm_poses':'user_data_arm_poses',
00326                            'planner_service_name':'user_data_planner_service_name',
00327                            'state':'user_data_state'})
00328 
00329         #Print goal for move_arm
00330         smach.StateMachine.add('ARM_POSES',
00331                 SimpleActionState('/iri_door_detector/door_detector_actions/find_a_door',
00332                                 iri_door_detector.msg.FindADoorAction,
00333                                 result_cb=find_a_door_arm_result_cb),
00334                 transitions={'succeeded_left':'GRASP_HANDLE_LEFT',
00335                              'succeeded_right':'GRASP_HANDLE_RIGHT',
00336                              'aborted':'FIND_A_DOOR_ARM'})
00337 
00338 
00339         #Lift arm to prepare the robot for partially open door crossing 
00340         smach.StateMachine.add('LIFT_ARM_LEFT',
00341                 SimpleActionState('move_left_arm',
00342                                 MoveArmAction,
00343                                 goal_slots=['planner_service_name',
00344                                             'motion_plan_request']),
00345                 transitions={'succeeded':'MOVE_BASE_OPEN',
00346                              'aborted':'SAFE_POSITION_LEFT_ARM'},
00347                 remapping={'motion_plan_request':'user_data_arm_poses',
00348                            'planner_service_name':'user_data_planner_service_name'})
00349 
00350         #Lift arm to prepare the robot for partially open door crossing 
00351         smach.StateMachine.add('LIFT_ARM_RIGHT',
00352                 SimpleActionState('move_right_arm_torso',
00353                                 MoveArmAction,
00354                                 goal_slots=['planner_service_name',
00355                                             'motion_plan_request']),
00356                 transitions={'succeeded':'MOVE_BASE_OPEN',
00357                              'aborted':'SAFE_POSITION_LEFT_ARM'},
00358                 remapping={'motion_plan_request':'user_data_arm_poses',
00359                            'planner_service_name':'user_data_planner_service_name'})
00360 
00361         #Move the arm above the handle
00362         smach.StateMachine.add('GRASP_HANDLE_LEFT',
00363                 SimpleActionState('move_left_arm',
00364                                 MoveArmAction,
00365                                 goal_slots=['planner_service_name',
00366                                             'motion_plan_request']),
00367                 transitions={'succeeded':'JOINT_STATE_LEFT_ARM',
00368                              'aborted':'FAIL_TURN_LEFT'},
00369                 remapping={'motion_plan_request':'user_data_arm_poses',
00370                            'planner_service_name':'user_data_planner_service_name'})
00371 
00372         #Joint poses
00373         smach.StateMachine.add('JOINT_STATE_LEFT_ARM', 
00374                                 CBState(joint_state_cb),
00375                 transitions={'succeeded':'TURN_HANDLE_LEFT',
00376                              'aborted':'FAIL_TURN_LEFT'})               
00377 
00378         #Move the arm above the handle
00379         smach.StateMachine.add('GRASP_HANDLE_RIGHT',
00380                 SimpleActionState('move_right_arm_torso',
00381                                 MoveArmAction,
00382                                 goal_slots=['planner_service_name',
00383                                             'motion_plan_request']),
00384                 transitions={'succeeded':'TURN_HANDLE_RIGHT',
00385                              'aborted':'FAIL_TURN_RIGHT'},
00386                 remapping={'motion_plan_request':'user_data_arm_poses',
00387                            'planner_service_name':'user_data_planner_service_name'})
00388 
00389         #Turn handle
00390         smach.StateMachine.add('TURN_HANDLE_RIGHT',
00391                 SimpleActionState('move_right_arm_torso',
00392                                 MoveArmAction,
00393                                 goal_cb=turn_handle_goal_cb,
00394                                 input_keys=['planner_service_name',
00395                                             'motion_plan_request']),
00396                 transitions={'succeeded':'PUSH_DOOR_RIGHT',
00397                              'aborted':'FAIL_TURN_RIGHT'},
00398                 remapping={'motion_plan_request':'user_data_arm_poses',
00399                            'planner_service_name':'user_data_planner_service_name'})
00400 
00401         #Turn handle
00402         smach.StateMachine.add('TURN_HANDLE_LEFT',
00403                 SimpleActionState('move_left_arm',
00404                                 MoveArmAction,
00405                                 goal_cb=turn_handle_goal_cb,
00406                                 input_keys=['planner_service_name',
00407                                             'motion_plan_request']),
00408                 transitions={'succeeded':'PUSH_DOOR_LEFT',
00409                              'aborted':'FAIL_TURN_LEFT'},
00410                 remapping={'motion_plan_request':'user_data_arm_poses',
00411                            'planner_service_name':'user_data_planner_service_name'})
00412 
00413         #Turn handle fail
00414         smach.StateMachine.add('FAIL_TURN_LEFT', 
00415                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00416                                 JointTrajectoryAction,
00417                                 goal_cb = rise_left_arm_cb),
00418                 transitions={'succeeded':'FIND_A_DOOR_ARM', 
00419                              'aborted':'FAIL_TURN_LEFT'})
00420 
00421         #Turn handle fail
00422         smach.StateMachine.add('FAIL_TURN_RIGHT', 
00423                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00424                                 JointTrajectoryAction,
00425                                 goal_cb = rise_right_arm_torso_cb),
00426                 transitions={'succeeded':'FIND_A_DOOR_ARM', 
00427                              'aborted':'FAIL_TURN_RIGHT'})
00428         
00429         #Push the door
00430         smach.StateMachine.add('PUSH_DOOR_LEFT',
00431                 SimpleActionState('move_base',
00432                                 MoveBaseAction,
00433                                 goal_cb=push_door_left_goal_cb),
00434                 transitions={'succeeded':'RELEASE_HANDLE_LEFT',
00435                              'aborted':'PUSH_DOOR_LEFT'})
00436 
00437         #Push the door
00438         smach.StateMachine.add('PUSH_DOOR_RIGHT',
00439                 SimpleActionState('move_base',
00440                                 MoveBaseAction,
00441                                 goal_cb=push_door_right_goal_cb),
00442                 transitions={'succeeded':'RELEASE_HANDLE_RIGHT',
00443                              'aborted':'PUSH_DOOR_RIGHT'})
00444 
00445         #Release handle
00446         smach.StateMachine.add('RELEASE_HANDLE_LEFT', 
00447                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00448                                 JointTrajectoryAction,
00449                                 goal_cb = rise_left_arm_cb),
00450                                 transitions={'succeeded':'TUCK_LEFT_ARM', 
00451                                              'aborted':'RELEASE_HANDLE_LEFT'})
00452         #Release handle
00453         smach.StateMachine.add('RELEASE_HANDLE_RIGHT', 
00454                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00455                                 JointTrajectoryAction,
00456                                 goal_cb = rise_right_arm_torso_cb),
00457                                 transitions={'succeeded':'TUCK_RIGHT_ARM', 
00458                                              'aborted':'RELEASE_HANDLE_RIGHT'})
00459 
00460         #Tuck arm
00461         smach.StateMachine.add('TUCK_LEFT_ARM', 
00462                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00463                                 JointTrajectoryAction,
00464                                 goal_cb = tuck_left_arm_cb),
00465                                 transitions={'succeeded':'STRETCH_RIGHT_ARM', 
00466                                              'aborted':'TUCK_LEFT_ARM'})
00467 
00468         #Tuck arm
00469         smach.StateMachine.add('TUCK_RIGHT_ARM', 
00470                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00471                                 JointTrajectoryAction,
00472                                 goal_cb = tuck_right_arm_torso_cb),
00473                                 transitions={'succeeded':'STRETCH_LEFT_ARM', 
00474                                              'aborted':'TUCK_RIGHT_ARM'})
00475         
00476         #Prepare the other arm to finish door push
00477         smach.StateMachine.add('STRETCH_LEFT_ARM', 
00478                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00479                                 JointTrajectoryAction,
00480                                 goal_cb = stretch_left_arm_cb),
00481                                 transitions={'succeeded':'SHRINK_LEFT_ARM', 
00482                                              'aborted':'STRETCH_LEFT_ARM'})
00483 
00484         #Prepare the other arm to finish door push
00485         smach.StateMachine.add('STRETCH_RIGHT_ARM', 
00486                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00487                                 JointTrajectoryAction,
00488                                 goal_cb = stretch_right_arm_torso_cb),
00489                                 transitions={'succeeded':'SHRINK_RIGHT_ARM', 
00490                                              'aborted':'STRETCH_RIGHT_ARM'})
00491 
00492         #Shrink arm
00493         smach.StateMachine.add('SHRINK_LEFT_ARM', 
00494                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00495                                 JointTrajectoryAction,
00496                                 goal_cb = tuck_left_arm_cb),
00497                                 transitions={'succeeded':'FINISH_PUSH_DOOR_RIGHT', 
00498                                              'aborted':'SHRINK_LEFT_ARM'})
00499 
00500         #Shrink arm
00501         smach.StateMachine.add('SHRINK_RIGHT_ARM', 
00502                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00503                                 JointTrajectoryAction,
00504                                 goal_cb = tuck_right_arm_torso_cb),
00505                                 transitions={'succeeded':'FINISH_PUSH_DOOR_LEFT', 
00506                                              'aborted':'SHRINK_RIGHT_ARM'})
00507 
00508         #Push the door
00509         smach.StateMachine.add('FINISH_PUSH_DOOR_LEFT',
00510                 SimpleActionState('move_base',
00511                                 MoveBaseAction,
00512                                 goal_cb=finish_push_door_left_goal_cb),
00513                 transitions={'succeeded':'REST_LEFT_ARM',
00514                              'aborted':'FINISH_PUSH_DOOR_LEFT'})
00515 
00516         #Push the door
00517         smach.StateMachine.add('FINISH_PUSH_DOOR_RIGHT',
00518                 SimpleActionState('move_base',
00519                                 MoveBaseAction,
00520                                 goal_cb=finish_push_door_right_goal_cb),
00521                 transitions={'succeeded':'REST_LEFT_ARM',
00522                              'aborted':'FINISH_PUSH_DOOR_RIGHT'})
00523 
00524         #Return to safe position
00525         smach.StateMachine.add('REST_LEFT_ARM', 
00526                 SimpleActionState('left_arm_controller/joint_trajectory_action',
00527                                 JointTrajectoryAction,
00528                                 goal_cb = init_left_arm_cb),
00529                                 transitions={'succeeded':'REST_RIGHT_ARM', 
00530                                              'aborted':'REST_LEFT_ARM'})
00531         #Return to safe position
00532         smach.StateMachine.add('REST_RIGHT_ARM', 
00533                 SimpleActionState('right_arm_torso_controller/joint_trajectory_action',
00534                                 JointTrajectoryAction,
00535                                 goal_cb = init_right_arm_torso_cb),
00536                                 transitions={'succeeded':'succeeded', 
00537                                              'aborted':'REST_RIGHT_ARM'})
00538 
00539 
00540     # Create and start the introspection server
00541     sis = smach_ros.IntrospectionServer('open_the_door', sm, '/SM_ROOT')
00542     sis.start() 
00543 
00544     # Execute SMACH plan
00545     outcome = sm.execute()
00546 
00547     # Wait for ctrl-c to stop the application
00548     rospy.spin()
00549     sis.stop()
00550 
00551 if __name__ == '__main__':
00552     main()
00553