transitions={'succeeded':'wait_sync1', 'aborted':'object_detection_2D'})
sm.add('wait_sync1', waitState.WaitState(0.0),
transitions={'succeeded':'object_detection_2D'})
sm.add('object_detection_2D', FindObject2DState(),
transitions={'no_objects':'decide_next_pick', 'succeeded':'decide_grasp_pose'})
sm.add('decide_grasp_pose', DecideGraspPoseState(),
transitions={'succeeded':'activate_suction', 'failed':'move_arm_to_bin'})
# activate suction here before moving the arm
sm.add('activate_suction',
PublisherState(topic='/robot/end_effector/right_gripper/command',
datatype=EndEffectorCommand,
# data={'command':'go', 'sender':'user', 'id': 65537, 'args':'{\"grip_attempt_seconds\": 100}'},
data={'command':'go', 'sender':'user', 'id': 65538, 'args':'{\"grip_attempt_seconds\": 100}'},
n=5, hz=10),
transitions={'succeeded':'grasp_object_in_bin', 'aborted':'decide_next_pick'})
# move the sucker into the object, colliding with it slightly
sm.add('grasp_object_in_bin', MoveRobotState(),
transitions={'succeeded':'wait_sucking', 'aborted':'decide_next_pick'})
sm.add('wait_sucking', waitState.WaitState(0.0),
transitions={'succeeded':'lift_object_from_bin'})
# lift the object up a little bit
sm.add('lift_object_from_bin', MoveRobotState(goal_frame_id='/shelf', goal_pose=Pose(position=Point(-0.05,0,0), orientation=Quaternion(0,0,0,1)), relative_motion=True),
transitions={'succeeded':'remove_object_from_bin'})
# decide which object to grasp next from which bin
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':'set_the_shelf_collision_scene',
'failed': 'aborted'})
# 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',
# =============================================================================
# =============================================================================
# =============================================================================
if __name__ == '__main__':
rospy.init_node('smach_shelf_reach_testing')
# Create the top level SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# create states and connect them
with sm:
# 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'})