def __init__(self): smach.State.__init__(self, outcomes=['Ready', 'Start'], input_keys=['tower_size_in'], output_keys=['tower_size_out']) self.jn = Jaco() self.jgn = JacoGripper()
def __init__(self,start,third_finger=True): smach.State.__init__(self, outcomes=['grasped', 'not_grasped']) self.finger_touch_sub = rospy.Subscriber('/finger_sensor/touch', FingerTouch, self.set_finger_touch) self.finger_touch = [None, None, None] self.jgn = JacoGripper() self.start = start self.third_finger = third_finger
def __init__(self, pos=None): smach.State.__init__(self, outcomes=['calibrated', 'not_calibrated']) self.calibrate_pub = rospy.Publisher( '/finger_sensor/calibrate_obj_det', Bool, queue_size=1) self.calibrated_sub = rospy.Subscriber( '/finger_sensor/obj_det_calibrated', Bool, self.set_calibrated) self.calibrated = None self.finger_pos = pos self.jgn = JacoGripper()
class StackingStart(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['Ready','Start'], input_keys=['tower_size_in'], output_keys=['tower_size_out']) self.jn = Jaco() self.jgn = JacoGripper() def execute(self,userdata): self.jgn.set_position([0.0,0.0,0.0]) if userdata.tower_size_in == 0: self.jn.home() userdata.tower_size_out = userdata.tower_size_in + 1 return 'Start' else: self.jn.home() userdata.tower_size_out = userdata.tower_size_in + 1 return 'Ready'
class CalibrateFingers(smach.State): def __init__(self, pos=None): smach.State.__init__(self, outcomes=['calibrated', 'not_calibrated']) self.calibrate_pub = rospy.Publisher( '/finger_sensor/calibrate_obj_det', Bool, queue_size=1) self.calibrated_sub = rospy.Subscriber( '/finger_sensor/obj_det_calibrated', Bool, self.set_calibrated) self.calibrated = None self.finger_pos = pos self.jgn = JacoGripper() def set_calibrated(self, msg): self.calibrated = msg.data def execute(self, userdata): if self.finger_pos: self.jgn.set_position(self.finger_pos) self.calibrate_pub.publish(True) time.sleep(5) while self.calibrated == False: pass return 'calibrated'
class GraspObject(smach.State): def __init__(self,start,third_finger=True): smach.State.__init__(self, outcomes=['grasped', 'not_grasped']) self.finger_touch_sub = rospy.Subscriber('/finger_sensor/touch', FingerTouch, self.set_finger_touch) self.finger_touch = [None, None, None] self.jgn = JacoGripper() self.start = start self.third_finger = third_finger def set_finger_touch(self,msg): self.finger_touch[0] = msg.finger1 self.finger_touch[1] = msg.finger2 self.finger_touch[2] = msg.finger3 def execute(self,userdata): status = self.jgn.close_with_feedback(self.start,self.third_finger) if np.any(np.array(self.finger_touch) == True) and status == 'done': return 'grasped' else: return 'not_grasped'
def main(): rospy.init_node("test") jg = JacoGripper() gripper_pos = [50.0, 50.0, 0.0] jg.set_position(gripper_pos) fingers_to_use = [0, 1] sm = smach.StateMachine(outcomes=['Success', 'Fail']) sm.userdata.tower_size = 0 with sm: smach.StateMachine.add('StackingStart', action_database.StackingStart(), transitions={ 'Start': 'PerceiveObjects1', 'Ready': 'GenerateCubeGrasp1' }, remapping={ 'tower_size_in': 'tower_size', 'tower_size_out': 'tower_size' }) smach.StateMachine.add('PerceiveObjects1', action_database.PerceiveObjects( ['unknown_1', 'unknown_0']), transitions={ 'objects_found': 'GenerateCubeGrasp1', 'objects_not_found': 'Success' }, remapping={ 'pick_obj_name_out': 'pick_object_name', 'place_obj_name_out': 'place_object_name' }) smach.StateMachine.add('GenerateCubeGrasp1', action_database.GenerateCubeGrasp(), transitions={ 'grasp_generated': 'CalibrateFingers1', 'grasp_not_generated': 'GenerateCubeGrasp1' }, remapping={ 'pick_obj_name_in': 'pick_object_name', 'place_obj_name_in': 'place_object_name', 'tower_size_in': 'tower_size' }) smach.StateMachine.add( 'CalibrateFingers1', action_database.CalibrateFingers(pos=gripper_pos), transitions={ 'calibrated': 'GotoObject1', 'not_calibrated': 'CalibrateFingers1' }) smach.StateMachine.add('GotoObject1', action_database.GotoObject('/pick_frame'), transitions={ 'there': 'CalibrateFingers2', 'no_tf_found': 'GotoObject1' }) smach.StateMachine.add('SearchObject3', action_database.SearchObject(fingers_to_use, search='down_z'), transitions={ 'found': 'GraspObject1', 'not_found': 'SearchObject1' }) smach.StateMachine.add('SearchObject1', action_database.SearchObject(fingers_to_use), transitions={ 'found': 'GraspObject1', 'not_found': 'GraspObject1' }) smach.StateMachine.add('GraspObject1', action_database.GraspObject(gripper_pos, third_finger=False), transitions={ 'grasped': 'GotoObject2', 'not_grasped': 'CalibrateFingers2' }) smach.StateMachine.add('GotoObject2', action_database.GotoObject('/place_frame'), transitions={ 'there': 'CalibrateFingers3', 'no_tf_found': 'GotoObject2' }) smach.StateMachine.add( 'CalibrateFingers2', action_database.CalibrateFingers(pos=gripper_pos), transitions={ 'calibrated': 'SearchObject3', 'not_calibrated': 'CalibrateFingers2' }) smach.StateMachine.add('CalibrateFingers3', action_database.CalibrateFingers(), transitions={ 'calibrated': 'SearchObject2', 'not_calibrated': 'CalibrateFingers3' }) smach.StateMachine.add('SearchObject2', action_database.SearchObject(fingers_to_use, search='down_z'), transitions={ 'found': 'StackingStart', 'not_found': 'StackingStart' }) outcome = sm.execute() jg.set_position([50.0, 50.0, 0.0])