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',
'aborted': 'repeat'
})
sm_init.add('decide_next_pick',
DecideNextPickItemState(),
transitions={
'succeeded': 'set_the_shelf_collision_scene_init',
'finished': 'abort_next_object',
'aborted': 'abort_next_object'
})
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'),
# goal_frame_id : a string with the TF frame of where to move the arm, used by MoveRobotState
# goal_pose : a 3D pose in the goal_frame_id frame, describing where to move the arm, used by MoveRobotState
# =============================================================================
# =============================================================================
# =============================================================================
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:
# 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={}),
# goal_pose : a 3D pose in the goal_frame_id frame, describing where to move the arm, used by MoveRobotState
# =============================================================================
# =============================================================================
# =============================================================================
if __name__ == '__main__':
rospy.init_node('smach_shelf_reach_testing')
# Create the top level SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
relative_movement = 0.33
# create states and connect them
with sm:
# decide which object to grasp next from which bin
sm.add('get_next_item', DecideNextPickItemState(),
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