Exemplo n.º 1
0
  def __init__(self):
  
    self.paused = False
    self.pauseCV = threading.Condition()
    
    # load the node's parameters
    self.arm_down_velocity = rospy.get_param('~arm_down_velocity', -0.5)
    self.arm_down_torque = rospy.get_param('~arm_down_torque', 0.5)
    self.arm_down_standoff = rospy.get_param('~arm_down_standoff', 0.1)
    
    self.arm_up_velocity = rospy.get_param('~arm_up_velocity', 0.5)
    self.arm_home_torque = rospy.get_param('~arm_home_torque', 0.4)
    self.arm_start_torque = rospy.get_param('~arm_start_torque', 0.9)
    self.arm_up_torque = rospy.get_param('~arm_up_torque', 0.6)
    self.arm_up_standoff = rospy.get_param('~arm_up_standoff', 0.1)
    
    #if arm position greater than this after heading down, we are too high for grabbing
    self.arm_down_maximum = rospy.get_param('~arm_down_maximum', 3.0)
    
    self.hand_open_position = rospy.get_param('~hand_open_position', 0.0)
    self.grip_torque = 0.5
    
    self.wrist_zero_position = rospy.get_param('~wrist_zero_position', 0.0)
    
    # wait for all the dynamixel services to actually come live.
    rospy.loginfo('Waiting for manipulator/wrist_joint services...')
    rospy.wait_for_service('wrist_joint/go_to_position')
  
    rospy.loginfo('Waiting for manipulator/arm_joint services...')
    rospy.wait_for_service('arm_joint/go_to_position')
  
    rospy.loginfo('Waiting for manipulator/hand_joint services...')
    rospy.wait_for_service('hand_joint/go_to_position')
  
    self.carousel_client = actionlib.SimpleActionClient('select_carousel_bin', SelectCarouselBinAction)    
    
    # make service proxies important services
    
    self.arm_velocity_standoff = rospy.ServiceProxy('arm_joint/velocity_standoff', VelocityStandoff)  
    self.wrist_go_to_position = rospy.ServiceProxy('wrist_joint/go_to_position', GoToPosition)  
    self.hand_go_to_position = rospy.ServiceProxy('hand_joint/go_to_position', GoToPosition)  
    self.arm_pause = rospy.ServiceProxy('arm_joint/pause', Enable)
    self.wrist_pause = rospy.ServiceProxy('wrist_joint/pause', Enable)
    self.hand_pause = rospy.ServiceProxy('hand_joint/pause', Enable)
    
    rospy.Subscriber('arm_joint/state',
                     JointState,
                     self.arm_joint_cb,
                     queue_size = 1 )
            
    #IMPORTANT - actions and services (including pause) will be
    #started after state machine declaration
    
    #create the state machine!
    self.sm = smach.StateMachine(
        outcomes=['success', 'aborted', 'preempted'],
        input_keys = ['action_goal'],
        output_keys = ['action_result']
    )
    
    self.sm.userdata.arm_down_maximum = self.arm_down_maximum
  
    with self.sm:
           
      smach.StateMachine.add('START_MANIPULATOR',
          manipulator_states.ProcessGoal(),
          transitions = {'grab': 'ROTATE_WRIST',
                         'home': 'HOME_ARM',
                         'preempted' : 'PAUSED',
                         'aborted':'ERROR'}
      )

      @smach.cb_interface()
      def manipulator_response_cb(userdata, response):
        return response.result
      
      @smach.cb_interface(input_keys=['wrist_angle'])
      def wrist_angle_cb(userdata, request):
        return GoToPositionRequest(self.wrist_zero_position + userdata.wrist_angle)
  
      smach.StateMachine.add('ROTATE_WRIST',
          smach_ros.ServiceState('wrist_joint/go_to_position', GoToPosition,
          request_cb = wrist_angle_cb,
          response_cb = manipulator_response_cb,
          input_keys = ['wrist_angle']),          
          transitions = {'succeeded':'ARM_DOWN',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )

      @smach.cb_interface(input_keys = ['arm_down_maximum',
                                        'arm_joint_position'],
                          output_keys = ['target_bin'],
                          outcomes = ['too_high'])
      def arm_down_response_cb(userdata, response):
        
        rospy.loginfo("MANIPULATOR ARM DOWN position: {!s}, limit: {!s}".format(userdata.arm_joint_position,
                                                                                userdata.arm_down_maximum))
        
        if userdata.arm_joint_position > userdata.arm_down_maximum:
          #don't give up and try to grab anyway, just report the info
          rospy.loginfo('MANIPULATOR did not get below arm_down_maximum, grabbing sample anyway')
          return 'too_high'
        else:
          return 'succeeded'

      smach.StateMachine.add('ARM_DOWN',
          smach_ros.ServiceState('arm_joint/velocity_standoff', VelocityStandoff,
          request = VelocityStandoffRequest(velocity = self.arm_down_velocity,
                                            start_torque_limit = self.arm_down_torque,                                                      
                                            torque_limit = self.arm_down_torque,
                                            distance = self.arm_down_standoff,
                                            check_velocity = False),
          response_cb = arm_down_response_cb),
          transitions = {'succeeded':'GRIP_SAMPLE',
                         'too_high':'GRIP_SAMPLE',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )
 
      @smach.cb_interface(input_keys=['grip_torque'])
      def grip_torque_cb(userdata, request):
        return TorqueHoldRequest(-1 * userdata.grip_torque)   
 
      smach.StateMachine.add('GRIP_SAMPLE',
          smach_ros.ServiceState('hand_joint/torque_hold', TorqueHold,
          request_cb = grip_torque_cb,
          response_cb = manipulator_response_cb,
          input_keys = ['grip_torque']),          
          transitions = {'succeeded':'ARM_UP',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )

      @smach.cb_interface(outcomes = ['timeout'])
      def arm_up_response_cb(userdata, response):
        return response.result

      smach.StateMachine.add('ARM_UP',
          smach_ros.ServiceState('arm_joint/velocity_standoff', VelocityStandoff,
          request = VelocityStandoffRequest(velocity = self.arm_up_velocity,
                                            start_torque_limit = self.arm_start_torque,                                                      
                                            torque_limit = self.arm_up_torque,
                                            distance = self.arm_up_standoff,
                                            check_velocity = True),
          response_cb = arm_up_response_cb),          
          transitions = {'succeeded':'GET_BIN',
                         'timeout':'RELEASE_GRIP',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      ) 
    
      smach.StateMachine.add('ZERO_WRIST',
          smach_ros.ServiceState('wrist_joint/go_to_position', GoToPosition,
          request = GoToPositionRequest(self.wrist_zero_position),
          response_cb = manipulator_response_cb),
          transitions = {'succeeded':'success',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      ) 
    
      @smach.cb_interface(input_keys=['target_bin'])
      def get_carousel_bin_cb(userdata, request):
        goal = SelectCarouselBinGoal()   
        goal.bin_index = userdata.target_bin
        return goal 
        
      #carousel bin is acquired from datastore via callback method
      smach.StateMachine.add('GET_BIN',
          smach_ros.SimpleActionState('select_carousel_bin',
          SelectCarouselBinAction,
          goal_cb=get_carousel_bin_cb,
          input_keys = ['target_bin']),
          transitions = {'succeeded':'OPEN_HAND',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )
 
      smach.StateMachine.add('OPEN_HAND',
          smach_ros.ServiceState('hand_joint/go_to_position', GoToPosition,
          request = GoToPositionRequest(self.hand_open_position),
          response_cb = manipulator_response_cb),          
          transitions = {'succeeded':'CLEAR_CAROUSEL',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )
  
      #return carousel to bin 0
      #this is always the last state before a successful state machine execution
      carousel_clear_bin = SelectCarouselBinGoal()
      carousel_clear_bin.bin_index = 0
      smach.StateMachine.add('CLEAR_CAROUSEL',
          smach_ros.SimpleActionState('select_carousel_bin',
          SelectCarouselBinAction,
          goal=carousel_clear_bin,
          output_keys=['action_result']),
          transitions = {'succeeded':'ZERO_WRIST',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )
      
      smach.StateMachine.add('PAUSED',
          manipulator_states.PausedState(self.pauseCV),
          transitions ={'aborted':'aborted',
                        'preempted':'preempted', #during a home action, return preempted immediately
                        'succeeded':'HOME_ARM'} #during a grab action, home after unpause
      )

      smach.StateMachine.add('HOME_ARM',
          smach_ros.ServiceState('arm_joint/velocity_standoff', VelocityStandoff,
          request = VelocityStandoffRequest(velocity = self.arm_up_velocity,
                                            start_torque_limit = self.arm_home_torque,                                                      
                                            torque_limit = self.arm_home_torque,
                                            distance = self.arm_up_standoff,
                                            check_velocity = False),
          response_cb = manipulator_response_cb,
          input_keys = ['error','goal']),
          transitions = {'succeeded':'HOME_WRIST',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      ) 

      smach.StateMachine.add('HOME_WRIST',
          smach_ros.ServiceState('wrist_joint/go_to_position', GoToPosition,
          request = GoToPositionRequest(self.wrist_zero_position),
          response_cb = manipulator_response_cb),
          transitions = {'succeeded':'HOME_HAND',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )

      smach.StateMachine.add('HOME_HAND',
          smach_ros.ServiceState('hand_joint/go_to_position', GoToPosition,
          request = GoToPositionRequest(self.hand_open_position),
          response_cb = manipulator_response_cb),          
          transitions = {'succeeded':'CLEAR_CAROUSEL',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )

      smach.StateMachine.add('RELEASE_GRIP',
          smach_ros.ServiceState('hand_joint/go_to_position', GoToPosition,
          request = GoToPositionRequest(self.hand_open_position),
          response_cb = manipulator_response_cb),          
          transitions = {'succeeded':'ARM_UP',
                         'preempted':'PAUSED',
                         'aborted':'ERROR'}
      )

      smach.StateMachine.add('ERROR',
          manipulator_states.ErrorState(),
          transitions = {'error':'aborted'}
      )      

    #now that the state machine is fully defined, make the action server wrapper
    manipulator_action_server = smach_ros.ActionServerWrapper(
        'grab_action', ManipulatorAction,
        wrapped_container = self.sm,
        succeeded_outcomes = ['success'],
        aborted_outcomes = ['aborted'],
        preempted_outcomes = ['preempted'],
        goal_key = 'action_goal',
        result_key = 'action_result'
    )
  
    sls = smach_ros.IntrospectionServer('smach_grab_introspection',
                                        self.sm,
                                        '/START_MANIPULATOR')
    sls.start()

    #subscribe to arm_joint state
    rospy.Subscriber('arm_joint/state',
                     JointState,
                     self.arm_joint_cb,
                     queue_size = 1 )
  
    #start action servers and services
    manipulator_action_server.run_server()
    self.pause_service = rospy.Service('pause', Enable, self.service_pause)
Exemplo n.º 2
0
 def get_carousel_bin_cb(userdata, request):
   goal = SelectCarouselBinGoal()   
   goal.bin_index = userdata.target_bin
   return goal