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