# fill unused bins with collision objects
# actions: fill_bins, unfill_bins
sm.add('set_the_shelf_collision_scene', ToggleBinFillersAndTote(action='fill_bins'),
transitions={'succeeded':'reset_kinfu',
'failed': 'aborted'})
sm.add('reset_kinfu',
PublisherState(topic='/ros_kinfu/reset',
datatype=Empty, data={}),
transitions={'succeeded':'wait_sync1', 'aborted':'aborted'})
sm.add('wait_sync1', waitState.WaitState(2.0),
transitions={'succeeded':'get_kinfu_cloud'})
sm.add('get_kinfu_cloud',GetKinfuCloud(),
transitions={'succeeded':'crop_kinfu_cloud','failed':'aborted'})
sm.add('crop_kinfu_cloud',ShelfBasedCropCloud(),
transitions={'succeeded':'detect_object','failed':'aborted'})
sm.add('detect_object', DetectObjectState(),
transitions={'no_objects':'decide_grasp_pose',
'succeeded':'decide_grasp_pose',
'confidence_too_low':'detect_object'}) # TODO add number of trials for the same object
sm.add('decide_grasp_pose', DecideGraspPoseStateFromPointCloud(),
transitions={'succeeded':'detect_object',
'failed':'detect_object'})
sm_perception.add('shelf_scan',
ScanShelfState(),
transitions={
'succeeded': 'toggle_lip_on',
'failed': 'repeat'
})
sm_perception.add('toggle_lip_on',
ToggleBinFillersAndTote(action='lip_on'),
transitions={
'succeeded': 'get_kinfu_cloud',
'failed': 'repeat'
})
sm_perception.add('get_kinfu_cloud',
GetKinfuCloud(),
transitions={
'succeeded': 'crop_kinfu_cloud',
'failed': 'repeat'
})
sm_perception.add('crop_kinfu_cloud',
ShelfBasedCropCloud(),
transitions={
'succeeded': 'segment_cloud',
'failed': 'repeat'
})
sm_perception.add('segment_cloud',
SegmentPointcloud(),
transitions={