# deactivate the suction, just to make sure that no errors since last run
sm.add('deactivate_suction_init',
PublisherState(topic='/robot/end_effector/right_gripper/command',
datatype=EndEffectorCommand,
data={'command':'stop', 'sender':'user', 'id': 65537},
n=5, hz=10),
transitions={'succeeded':'decide_next_pick'})
sm.add('decide_next_pick', DecideNextPickItemState(),
transitions={'succeeded':'reset_the_shelf_collision_scene_init', 'finished': 'succeeded'})
sm.add('reset_the_shelf_collision_scene_init', ToggleBinFillersAndTote(action='fill_bins'),
transitions={'succeeded':'move_to_neutral_start',
'failed': 'abort_state'})
sm.add('move_to_neutral_start', MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_neutral'),
transitions={'succeeded':'move_arm_to_look_into_bin', # 'lift_object_up'
'failed': 'abort_state'})
# sm.add('remove_object_collision', UpdateCollisionState(action='detach'),
# transitions={'succeeded':'set_the_shelf_collision_scene'})
# fill unused bins with collision objects
# actions: fill_bins, unfill_bins
# sm.add('set_the_shelf_collision_scene', ToggleBinFillersAndTote(action='fill_bins'),
# transitions={'succeeded':'set_the_tote_collision_scene',
# 'failed': 'abort_state'})
#
# # add tote and tote pillar to collision scene
# # tote/pillar actions: tote_on, tote_off, pillar_on, pillar_off, all_on, all_off
# sm.add('set_the_tote_collision_scene', ToggleBinFillersAndTote(action='all_on'),
sm_init.add('set_the_shelf_collision_scene_init',
ToggleBinFillersAndTote(action='fill_bins'),
transitions={
'succeeded': 'set_the_tote_collision_scene',
'failed': 'repeat'
})
sm_init.add('set_the_tote_collision_scene',
ToggleBinFillersAndTote(action='all_on'),
transitions={
'succeeded': 'move_to_neutral_start',
'failed': 'repeat'
})
sm_init.add('move_to_neutral_start',
MoveRobotToNamedPose(movegroup=limb + 'arm',
goal_pose_name=limb + 'neutral'),
transitions={
'succeeded': 'perception',
'failed': 'repeat'
})
sm_perception = smach.StateMachine(
outcomes=['repeat', 'grasping', 'abort_next_object'],
input_keys=['next_item_to_pick'],
output_keys=['googlenet'])
with sm_perception:
sm_perception.add('toggle_lip_off',
ToggleBinFillersAndTote(action='lip_off'),
transitions={
sm_init.add('turn_on_tote_and_podium_2',
ToggleBinFillersAndTote(action='all_on'),
transitions={
'succeeded': 'set_the_shelf_collision_scene',
'failed': 'turn_on_tote_and_podium_2'
})
sm_init.add('set_the_shelf_collision_scene',
ToggleBinFillersAndTote(action='fill_bins_tote'),
transitions={
'succeeded': 'move_to_look_into_tote_pose',
'failed': 'set_the_shelf_collision_scene'
})
sm_init.add('move_to_left_tote',
MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_tote'),
transitions={
'succeeded': 'move_to_look_into_tote_pose',
'aborted': 'repeat',
'failed': 'move_to_left_tote'
})
sm_init.add('move_to_look_into_tote_pose',
MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='look_into_tote'),
transitions={
'succeeded': 'perception',
'aborted': 'repeat',
'failed': 'move_to_look_into_tote_pose'
})
sm.add('set_the_next_bin', SetNextBin(action='nothing'),
transitions={'succeeded':'set_the_shelf_collision_scene',
'failed': 'abort_state'})
sm.add('set_the_shelf_collision_scene', ToggleBinFillersAndTote(action='fill_bins'),
transitions={'succeeded':'set_the_tote_collision_scene',
'failed': 'abort_state'})
# add tote and tote pillar to collision scene
# tote/pillar actions: tote_on, tote_off, pillar_on, pillar_off, all_on, all_off
sm.add('set_the_tote_collision_scene', ToggleBinFillersAndTote(action='all_on'),
transitions={'succeeded':'move_to_neutral_start',
'failed': 'abort_state'})
#move to a neutral start
sm.add('move_to_neutral_start', MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_neutral'),
transitions={'succeeded':'move_arm_to_look_into_bin', # 'lift_object_up'
'failed': 'abort_state'})
# move arm in front of bin before looking for the object
sm.add('get_target_bin_coordinates', GetBinCoordinatesState(),
transitions={'succeeded':'move_arm_to_look_into_bin','aborted':'abort_state'})
#
# sm.add('move_arm_to_look_into_bin', MoveRobotState(movegroup='left_arm_realsense'),
sm.add('move_arm_to_look_into_bin', MoveRobotToNamedPose(movegroup='left_arm',
name_prefix='look_into_'),
transitions={'succeeded':'reset_kinfu',
'failed': 'abort_state',
'aborted':'abort_state'})
sm.add('reset_kinfu',
DecideNextPickItemState(),
transitions={
'succeeded': 'reset_the_shelf_collision_scene_init',
'finished': 'succeeded'
})
sm.add('reset_the_shelf_collision_scene_init',
ToggleBinFillersAndTote(action='fill_bins'),
transitions={
'succeeded': 'move_to_neutral_start',
'failed': 'abort_state'
})
sm.add(
'move_to_neutral_start',
MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_neutral'),
transitions={
'succeeded': 'remove_object_collision', # 'lift_object_up'
'failed': 'abort_state'
})
sm.add('remove_object_collision',
UpdateCollisionState(action='detach'),
transitions={'succeeded': 'set_the_shelf_collision_scene'})
# fill unused bins with collision objects
# actions: fill_bins, unfill_bins
sm.add('set_the_shelf_collision_scene',
ToggleBinFillersAndTote(action='fill_bins'),
transitions={
'succeeded': 'set_the_tote_collision_scene',
transitions={'succeeded':'set_the_shelf_collision_scene', 'finished': 'succeeded'})
# transitions={'succeeded':'add_object_collision', 'finished': 'succeeded'})
sm.add('set_the_shelf_collision_scene', ToggleBinFillersAndTote(action='fill_bins'),
transitions={'succeeded':'set_the_tote_collision_scene',
'failed': 'abort_state'})
# add tote and tote pillar to collision scene
# tote/pillar actions: tote_on, tote_off, pillar_on, pillar_off, all_on, all_off
sm.add('set_the_tote_collision_scene', ToggleBinFillersAndTote(action='all_on'),
transitions={'succeeded':'move_to_tote',
'failed': 'abort_state'})
# move your hand above the tote to look into it
sm.add('move_to_tote', MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='look_into_tote'),
# transitions={'succeeded':'move_arm_to_object',
transitions={'succeeded':'detect_object',
'failed': 'abort_state'})
sm.add('detect_object', DetectObjectState(),
transitions={'no_objects':'detect_object',
'succeeded':'turn_tote_collision_off',
'confidence_too_low':'detect_object'}) # TODO add number of trials for the same object
# 'succeeded':'move_arm_infront_of_object'})
sm.add('turn_tote_collision_off', ToggleBinFillersAndTote(action='tote_off'),
transitions={'succeeded':'decide_grasp_pose',
'failed': 'abort_state'})