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'})