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)
def get_carousel_bin_cb(userdata, request): goal = SelectCarouselBinGoal() goal.bin_index = userdata.target_bin return goal