def main(): rospy.init_node('smach_ranking_controller') bumper_sub = rospy.Subscriber("bumper", Bumper, bumper_cb) cliff_sub = rospy.Subscriber("cliff_msg", Cliff, cliff_cb) virtual_wall_sub = rospy.Subscriber("virtual_wall", Bool, virtual_wall_cb) global vel_pub vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) rospy.on_shutdown(stop_cb) # Create a SMACH state machine sm = smach.StateMachine(outcomes=['aborted']) # Open the container with sm: # Add states to the container smach.StateMachine.add('FORWARD', Forward(), transitions={ 'left_pressed': 'ROTATE_RIGHT_SEQ', 'right_pressed': 'ROTATE_LEFT_SEQ', 'both_pressed': 'ROTATE_LEFT_SEQ', 'virtual_wall_detected': 'ROTATE_LEFT_SEQ', 'cliff_left': 'ROTATE_RIGHT_SEQ', 'cliff_right': 'ROTATE_LEFT_SEQ' }) smach.StateMachine.add('BACKWARD', Backward(), transitions={'done': 'FORWARD'}) right_sq = smach.Sequence(outcomes=['done', 'aborted'], connector_outcome='done') with right_sq: smach.Sequence.add('BACKWARD', Backward()) smach.Sequence.add('ROTATE_RIGHT', RotateRight()) left_sq = smach.Sequence(outcomes=['done', 'aborted'], connector_outcome='done') with left_sq: smach.Sequence.add('BACKWARD', Backward()) smach.Sequence.add('ROTATE_LEFT', RotateLeft()) smach.StateMachine.add('ROTATE_RIGHT_SEQ', right_sq, transitions={'done': 'FORWARD'}) smach.StateMachine.add('ROTATE_LEFT_SEQ', left_sq, transitions={'done': 'FORWARD'}) # Create and start the introspection server sis = smach_ros.IntrospectionServer('smach_introspection', sm, '/SM_ROOT') sis.start() # Execute SMACH plan _ = sm.execute() rospy.spin() sis.stop()
def main(): rospy.init_node('smach_example_state_machine') # Create a SMACH state machine #sm = smach.StateMachine(outcomes = ['succeeded', 'aborted', 'preempted']) sm = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded') # Open the container with sm: ''' smach.StateMachine.add('FOO', Foo(), transitions={'succeeded':'BAR'}) smach.StateMachine.add('BAR', Bar(), transitions={'succeeded':'BAS'}) smach.StateMachine.add('BAS', Bas()) ''' smach.Sequence.add('FOO', Foo()) smach.Sequence.add('BAR', Bar()) smach.Sequence.add('BAS', Bas()) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() rospy.spin() sis.stop()
def make_hydrophones(shared): sm_travel = smach.StateMachine(['succeeded', 'preempted']) with sm_travel: smach.StateMachine.add('HYDROPHONES', hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE), transitions={'failed': 'GO_AWAY'}) if constants.MODE == 'competition': smach.StateMachine.add('GO_AWAY', common_states.WaypointSeriesState(shared, [lambda cur: cur.set_orientation(orientation_helpers.NORTH).turn_right_deg(22.5), lambda cur: cur.forward(5)]), transitions={'succeeded': 'HYDROPHONES'}) else: smach.StateMachine.add('GO_AWAY', common_states.WaypointSeriesState(shared, [lambda cur: cur.set_orientation(orientation_helpers.NORTH).turn_left_deg(22.5), lambda cur: cur.forward(5)]), transitions={'succeeded': 'HYDROPHONES'}) sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add('RETRACT', subjugator_states.GasPoweredStickState(False)) smach.Sequence.add('DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(APPROACH_DEPTH))) smach.Sequence.add('TRAVEL', sm_travel) return sm
def main(): rospy.init_node('sekvenca1') # Create a SMACH state machine sm = smach.StateMachine(['succeeded','aborted','preempted']) # Open the container with sm: # Wake up robot smach.StateMachine.add('Wake_up', ServiceState('/pepper_robot/pose/wakeup', Empty), transitions={'succeeded':'', 'aborted':'aborted', 'preempted':'preempted'}) sq = smach.Sequence( outcomes = ['succeeded','aborted','preempted'], connector_outcome = 'succeeded') with sq: smach.Sequence.add('MOVE_ARM_GRAB_PRE', MoveVerticalGripperPoseActionState()) smach.Sequence.add('MOVE_GRIPPER_OPEN', MoveGripperState(GRIPPER_MAX_WIDTH)) smach.Sequence.add('MOVE_ARM_GRAB', MoveVerticalGripperPoseActionState()) smach.Sequence.add('MOVE_GRIPPER_CLOSE', MoveGripperState(grab_width)) smach.Sequence.add('MOVE_ARM_GRAB_POST', MoveVerticalGripperPoseActionState()) # sekvenca1: # tts # pokret koji je sprzen u chorographu #pokret koji je sekvenca za gripper # kombinacija sinkronizacija tts+ pokret #govor preko servisa pozvati, # Go to rest pose smach.StateMachine.add('Rest', ServiceState('/pepper_robot/pose/rest', Empty), transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def make_surface(shared): sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add( 'SURFACE', common_states.WaypointState(shared, lambda cur: cur.depth(-.1))) smach.Sequence.add('SLEEP', common_states.SleepState(5)) smach.Sequence.add( 'DOWN', common_states.WaypointState(shared, lambda cur: cur.depth(.3))) return sm
def create(): """Creates the launch state machine. :return The launch sequence machine object """ sm_launch = smach.Sequence(outcomes=['succeeded', 'preempted', 'failed'], connector_outcome='succeeded') with sm_launch: smach.Sequence.add('WAIT FOR GAIT SERVER', WaitForGaitServerState()) return sm_launch
def make_grabber_drop(shared): sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(2))) smach.Sequence.add('OPEN_GRABBER', subjugator_states.OpenGrabberState()) smach.Sequence.add( 'UP_DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(.5))) return sm
def make_starting_gate(shared): sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add('GO', common_states.WaypointSeriesState(shared, [lambda cur: cur.depth(constants.PIPE_DEPTH), lambda cur: cur.forward(10)])) smach.Sequence.add('APPROACH', common_states.VelocityState(shared, numpy.array([constants.PIPE_SPEED, 0, 0]))) smach.Sequence.add('WAIT_PIPE', legacy_vision_states.WaitForObjectsState(shared, 'find2_down_camera', 'pipe'), transitions={'timeout': 'failed'}) return sm
def make_bins_return(shared): sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add('RETURN', common_states.ReturnToWaypointState(shared, 'bins')) smach.Sequence.add('WAIT_BINS', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'bins/all', timeout=5), transitions={'timeout': 'failed'}) smach.Sequence.add( 'CENTER_ALL', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera')) smach.Sequence.add( 'ALIGN_ALL', legacy_vision_states.AlignObjectState(shared, 'find2_down_camera', body_vec_align=[0, 1, 0])) smach.Sequence.add( 'CENTER_ALL_2', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera')) smach.Sequence.add('TURN_DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(constants.PIPE_DEPTH)\ .turn_left_deg(90))) smach.Sequence.add( 'APPROACH', common_states.VelocityState( shared, numpy.array([constants.PIPE_SPEED, 0, 0]))) smach.Sequence.add('WAIT_PIPE', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'pipe'), transitions={'timeout': 'failed'}) selector = legacy_vision_states.select_by_body_direction( [0, 1 if constants.MODE == 'competition' else -1, 0]) smach.Sequence.add( 'CENTER_PIPE', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera', selector)) smach.Sequence.add( 'TURN_HELP_PIPE', common_states.WaypointState( shared, lambda cur: cur.turn_left_deg(45) if constants.MODE == 'competition' else cur.turn_right_deg(45))) return sm
def main(): rospy.init_node("smach_example_sequence") # Create a SMACH state machine sq = smach.Sequence(outcomes=["succeeded"], connector_outcome="done") # Open the container with sq: # Add states to the container smach.Sequence.add("FOO", ExampleState()) smach.Sequence.add("BAR", ExampleState()) smach.Sequence.add("BAZ", ExampleState(), {"done": "succeeded"}) # Execute SMACH plan outcome = sq.execute()
def main(): rospy.init_node('smach_example_sequence') # Create a SMACH state machine sq = smach.Sequence(outcomes=['succeeded'], connector_outcome='done') # Open the container with sq: # Add states to the container smach.Sequence.add('FOO', ExampleState()) smach.Sequence.add('BAR', ExampleState()) smach.Sequence.add('BAZ', ExampleState(), {'done': 'succeeded'}) # Execute SMACH plan outcome = sq.execute()
def _test_look_around(): rospy.init_node('smach') print POSES print POSES_GLIMPSE print POSES_CRAZY sq = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded') with sq: smach.Sequence.add('LOOK_AROUND', get_lookaround_smach(util.SleepState(1))) smach.Sequence.add('GLIMPSE_AROUND', get_lookaround_smach(util.SleepState(1), glimpse=True)) util.execute_smach_container(sq, enable_introspection=True)
def get_lookaround_smach(interjacent_state=None, glimpse=False, crazy=False): """if the interjacent state is not omitted its outcomes must include 'succeeded' but can also be 'aborted' or 'preempted'. """ sq = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded') poses = POSES_GLIMPSE if glimpse else POSES_CRAZY if crazy else POSES with sq: for i in range(len(poses)): sq.add('LOOKAROUND_MOVE_ARM_%d' % i, move_arm.get_move_arm_to_joints_positions_state(poses[i])) if interjacent_state is not None: sq.add('LOOKAROUND_INTERJACENT_STATE_%d' % i, interjacent_state) return sq
def make_hydrophones_close(shared): sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add('DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(1.5))) smach.Sequence.add('HYDROPHONES_TRAVEL', hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE), transitions={'failed': 'HYDROPHONES_TRAVEL'}) smach.Sequence.add('HYDROPHONES_APPROACH', hydrophone_states.HydrophoneApproachState(shared, FREQ, FREQ_RANGE), transitions={'failed': 'succeeded'}) # Ensure we drop smach.Sequence.add('BACKUP', common_states.WaypointState(shared, lambda cur: cur.backward(.3))) return sm
def gen_sm_vis_exec(visualize=True, confirm=True, execute=True): sm = smach.Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') sm.register_input_keys(['trajectory']) msg = """Press key: [y = continue, r = repeat, a = abort]: """ qe = GenericAskState(msg, answers={ "y": "succeeded", "a": "aborted", "r": "repeat" }) with sm: if visualize: Sequence.add("VISUALIZE", TrajectoryVisualizeState()) if confirm and visualize: Sequence.add("Q_EXECUTE", qe, transitions={'repeat': 'VISUALIZE'}) if execute: Sequence.add("EXECUTE", TrajectoryExecuteState()) return sm
def main(): rospy.init_node('smach_example_sequence') # Create a SMACH state machine sq = smach.Sequence(outcomes=['succeeded'], connector_outcome='done') # Open the container with sq: # Add states to the container smach.Sequence.add('FOO', ExampleState()) smach.Sequence.add('BAR', ExampleState()) smach.Sequence.add('BAZ', ExampleState(), {'done': 'succeeded'}) # Execute SMACH plan sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT') sis.start() ## Execute SMACH plan outcome = sq.execute() rospy.spin() sis.stop()
def explore_house_sm(): """ Explore house SM: - segment map into rooms and plan visit sequence - iterate over all rooms and explore following the planned sequence """ # segment house into rooms and plan visit sequence plan_room_seq_sm = smach.Sequence( outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded', input_keys=[ 'map_image', 'map_resolution', 'map_origin', 'robot_radius', 'segmented_map', 'room_information_in_meter', 'room_information_in_pixel' ], output_keys=['room_sequence']) with plan_room_seq_sm: smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose()) smach.Sequence.add('PLAN_ROOM_SEQUENCE', PlanRoomSequence(), remapping={ 'robot_start_coordinate': 'robot_pose', 'input_map': 'map_image' }) # explore a single room explore_1_room_sm = smach.Sequence( outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded', input_keys=[ 'map_image', 'map_resolution', 'map_origin', 'robot_radius', 'segmented_map', 'room_number', 'room_information_in_meter' ]) with explore_1_room_sm: smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose()) smach.Sequence.add('PLAN_ROOM_EXPL', PlanRoomExploration(), transitions={ 'aborted': 'aborted', 'preempted': 'preempted' }) smach.Sequence.add('POSES_AS_PATH', PosesAsPath(), remapping={'poses': 'coverage_path_pose_stamped'}) smach.Sequence.add( 'GOTO_START_POSE', GoToPose( dist_tolerance=cfg.LOOSE_DIST_TOLERANCE, # just close enough angle_tolerance=cfg.INF_ANGLE_TOLERANCE), # ignore orientation transitions={ 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'target_pose': 'start_pose'}) smach.Sequence.add('TRAVERSE_POSES', ExeSparsePath()) # iterate over all rooms and explore following the planned sequence explore_house_it = smach.Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[ 'map_image', 'map_resolution', 'map_origin', 'robot_radius', 'segmented_map', 'room_sequence', 'room_information_in_meter' ], output_keys=[], it=lambda: sm.userdata. room_sequence, # must be a lambda because we destroy the list TODO ehhh??? it_label='room_number', exhausted_outcome='succeeded') with explore_house_it: smach.Iterator.set_contained_state( 'EXPLORE_1_ROOM', explore_1_room_sm, loop_outcomes=['succeeded', 'aborted']) # Full SM: plan rooms visit sequence and explore each room in turn sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with sm: smach.StateMachine.add('HAVE_SEGMENTED_MAP', UDHasKey('segmented_map'), transitions={ 'true': 'PLAN_ROOM_SEQ', 'false': 'SEGMENT_ROOMS' }) smach.StateMachine.add('SEGMENT_ROOMS', SegmentRooms(), transitions={ 'succeeded': 'PLAN_ROOM_SEQ', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('PLAN_ROOM_SEQ', plan_room_seq_sm, transitions={ 'succeeded': 'EXPLORE_HOUSE', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('EXPLORE_HOUSE', explore_house_it, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) return sm
def make_manipulation(shared): traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory') manipulation_desc = TargetDesc() manipulation_desc.type = TargetDesc.TYPE_OBJECT manipulation_desc.object_filename = roslib.packages.resource_file( 'auvsi_robosub', 'models', '2013/manipulation.obj') manipulation_desc.prior_distribution.pose.orientation = Quaternion( *traj.turn_left_deg(180).orientation) manipulation_desc.disallow_yawing = True manipulation_desc.min_dist = BOARD_DIST - 1 manipulation_desc.max_dist = BOARD_DIST + 3 wheel = TargetDesc() wheel.type = TargetDesc.TYPE_OBJECT wheel.object_filename = roslib.packages.resource_file( 'auvsi_robosub', 'models', '2013/manipulation_wheel.obj') wheel.prior_distribution.pose.orientation = Quaternion( *traj.turn_left_deg(180).orientation) cov = numpy.zeros((6, 6)) a = numpy.array([traj.forward_vector]).T * 100 cov[3:, 3:] += a.dot(a.T) wheel.prior_distribution.covariance = cov.flatten() wheel.min_dist = WHEEL_DIST * .9 wheel.max_dist = WHEEL_DIST / .9 wheel.allow_rolling = True wheel.disallow_yawing = True # Create a SMACH state machine sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'], 'succeeded') # Open the container with sm: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(1.5))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add( 'WAIT_MANIPULATION', object_finder_states.WaitForObjectsState( shared, 'find_forward', lambda: [manipulation_desc], .95)) smach.Sequence.add( 'STOP', common_states.VelocityState(shared, numpy.array([0, 0, 0]))) smach.Sequence.add('SLEEP', common_states.SleepState(5)) smach.Sequence.add( 'APPROACH_MANIPULATION', object_finder_states.ApproachObjectState(shared, 'find_forward', 'forward_camera', BOARD_DIST)) smach.Sequence.add( 'OPEN_LOOP_FORWARD', common_states.WaypointState( shared, lambda cur: cur.forward(BOARD_DIST - WHEEL_DIST).left(.5))) smach.Sequence.add( 'WAIT_WHEEL', object_finder_states.WaitForObjectsState(shared, 'find_forward', lambda: [wheel], .99)) smach.Sequence.add('WAIT_WHEEL_MORE', common_states.SleepState(5)) smach.Sequence.add( 'APPROACH_WHEEL', object_finder_states.ApproachObjectState(shared, 'find_forward', 'forward_camera', WHEEL_DIST)) smach.Sequence.add('EXTEND', subjugator_states.GasPoweredStickState(True)) smach.Sequence.add('OPEN_LOOP_FORWARD2', common_states.WaypointState(shared, lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\ .relative([0, .06, .06]))) smach.Sequence.add( 'TURN', common_states.WaypointSeriesState(shared, [ lambda cur: cur.down(.2), lambda cur: cur.right(.2), lambda cur: cur.up(.2), lambda cur: cur.left(.2), lambda cur: cur.down(.2), lambda cur: cur.right(.2) ])) smach.Sequence.add('RETRACT', subjugator_states.GasPoweredStickState(False)) smach.Sequence.add( 'BACKUP', common_states.WaypointState(shared, lambda cur: cur.backward(1))) return sm
def make_bins(shared): sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_approach: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(CENTER_DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add('WAIT_ALL', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'bins/all'), transitions={'timeout': 'failed'}) smach.Sequence.add('EXTRA_FORWARD', common_states.SleepState(2)) smach.Sequence.add( 'RIGHT_TWEAK', common_states.WaypointState(shared, lambda cur: cur.turn_right_deg(25))) sm_center = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_center: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(CENTER_DEPTH))) smach.Sequence.add('WAIT_ALL', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'bins/all', timeout=10), transitions={'timeout': 'failed'}) smach.Sequence.add( 'CENTER_ALL', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera')) smach.Sequence.add( 'ALIGN_ALL', legacy_vision_states.AlignObjectState(shared, 'find2_down_camera', body_vec_align=[0, 1, 0])) smach.Sequence.add( 'CENTER_ALL_2', legacy_vision_states.CenterObjectState(shared, 'find2_down_camera')) smach.Sequence.add('SAVE_POS', common_states.SaveWaypointState('bins')) sm_drops = [] for bin_string in DROP_ORDER: sm_drop = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') sm_drops.append(sm_drop) with sm_drop: # this could be faster if CenterObjectState let you descend at the same time smach.Sequence.add('WAIT_SINGLE', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'bins/single', timeout=10), transitions={'timeout': 'failed'}) selector = select_image_text_or_most_central(bin_string) smach.Sequence.add( 'APPROACH_SINGLE', legacy_vision_states.CenterApproachObjectState( shared, 'find2_down_camera', desired_scale=9000, selector=selector)) smach.Sequence.add( 'DOWN', common_states.WaypointState( shared, lambda cur: cur.depth(3).backward(.15))) smach.Sequence.add('SLEEP', common_states.SleepState(2)) smach.Sequence.add('DROP', subjugator_states.DropBallState()) smach.Sequence.add('SLEEP2', common_states.SleepState(2)) sm = smach.StateMachine(['succeeded', 'failed', 'preempted']) with sm: smach.StateMachine.add('APPROACH', sm_approach, transitions={'succeeded': 'CENTER_1'}) smach.StateMachine.add('CENTER_1', sm_center, transitions={ 'succeeded': 'DROP_1', 'failed': 'APPROACH' }) smach.StateMachine.add('DROP_1', sm_drops[0], transitions={ 'succeeded': 'CENTER_2', 'failed': 'RETRY_CENTER_1' }) smach.StateMachine.add('RETRY_CENTER_1', common_states.CounterState(1), transitions={ 'succeeded': 'CENTER_1', 'exceeded': 'CENTER_2' }) smach.StateMachine.add('CENTER_2', sm_center, transitions={'succeeded': 'DROP_2'}) smach.StateMachine.add('DROP_2', sm_drops[1], transitions={ 'succeeded': 'succeeded', 'failed': 'RETRY_CENTER_2' }) smach.StateMachine.add('RETRY_CENTER_2', common_states.CounterState(1), transitions={ 'succeeded': 'CENTER_2', 'exceeded': 'succeeded' }) return sm
def make_shooter(shared): # Create a SMACH state machine sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_approach: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.1, 0, 0]))) sm_shoots = [] for color, shooter in zip(COLORS, ['left', 'right']): sm_shoot = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') sm_shoots.append(sm_shoot) with sm_shoot: # smach.Sequence.add('WAIT_SHOOTER', # legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera', # '/'.join(['shooter', color, 'box'])), # transitions={'timeout': 'failed'}) # smach.Sequence.add('APPROACH_SHOOTER', # legacy_vision_states.CenterApproachObjectState(shared, 'find2_forward_camera', # desired_scale=BOARD_SCALE)) smach.Sequence.add('WAIT_HEXAGON', legacy_vision_states.WaitForObjectsState( shared, 'find2_forward_camera', '/'.join(['shooter_flood', color, SIZE]), timeout=15), transitions={'timeout': 'failed'}) smach.Sequence.add( 'APPROACH_HEXAGON', legacy_vision_states.CenterApproachObjectState( shared, 'find2_forward_camera', desired_scale=HEXAGON_SCALE, gain=.5)) if shooter == 'left': smach.Sequence.add('OPEN_LOOP_FORWARD', common_states.WaypointState(shared, lambda cur: cur.forward(ALIGN_FORWARD)\ .right(ALIGN_STRAFE)\ .up(ALIGN_UP))) else: smach.Sequence.add('OPEN_LOOP_FORWARD', common_states.WaypointState(shared, lambda cur: cur.forward(ALIGN_FORWARD)\ .left(ALIGN_STRAFE)\ .up(ALIGN_UP))) smach.Sequence.add('SLEEP', common_states.SleepState(3)) smach.Sequence.add('SHOOT', subjugator_states.ShootTorpedoState(shooter)) smach.Sequence.add('SLEEP2', common_states.SleepState(3)) sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_retreat: smach.Sequence.add( 'RETREAT', common_states.VelocityState(shared, numpy.array([-.2, 0, 0]))) smach.Sequence.add('WAIT', common_states.SleepState(4)) sm = smach.StateMachine(['succeeded', 'failed', 'preempted']) with sm: smach.StateMachine.add('APPROACH_SHOOT_1', sm_approach, transitions={'succeeded': 'SHOOT_1'}) smach.StateMachine.add('RETREAT_SHOOT_1', sm_retreat, transitions={'succeeded': 'SHOOT_1'}) smach.StateMachine.add('SHOOT_1', sm_shoots[0], transitions={ 'succeeded': 'RETREAT_SHOOT_2', 'failed': 'RETREAT_SHOOT_1' }) smach.StateMachine.add('RETREAT_SHOOT_2', sm_retreat, transitions={'succeeded': 'SHOOT_2'}) smach.StateMachine.add('SHOOT_2', sm_shoots[1], transitions={ 'succeeded': 'DONE_BACKUP', 'failed': 'RETREAT_SHOOT_2' }) smach.StateMachine.add('DONE_BACKUP', common_states.WaypointState( shared, lambda cur: cur.backward(.5)), transitions={'succeeded': 'succeeded'}) return sm
def main(): rospy.init_node('commander') com = smach.StateMachine(outcomes=['succeeded','aborted','preempted']) with com: smach.StateMachine.add('ARM', smach_ros.SimpleActionState('arm_action', DoArmAction, goal=DoArmGoal(arm_cmd=DoArmGoal.ARM)), {'succeeded':'WAYPOINTS', 'aborted':'aborted'}) waypoint_sequence = smach.Sequence( outcomes=['succeeded','aborted','preempted'], connector_outcome='succeeded') with waypoint_sequence: smach.Sequence.add('TAKEOFF',smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP0'])) smach.Sequence.add('WP1',smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP1'])) smach.Sequence.add('WP2',smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP2'])) smach.Sequence.add('WP3',smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP3'])) smach.Sequence.add('WP4',smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP0'])) smach.StateMachine.add('WAYPOINTS',waypoint_sequence, transitions={'succeeded':'TRACK', 'preempted':'preempted', 'aborted':'aborted'}) track_machine = smach.Concurrence(outcomes=['locked','notready'], default_outcome='notready', outcome_map={'locked': { 'COV_MONITOR':'invalid', 'SEEK':'succeeded'}}) with track_machine: smach.Concurrence.add('SEEK',smach_ros.SimpleActionState('track_action', DoTrackAction, goal=DoTrackGoal(alt_sp=15))) smach.Concurrence.add('COV_MONITOR',smach_ros.MonitorState('odometry/filtered', Odometry, covariance_cb)) smach.StateMachine.add('TRACK',track_machine, transitions={'notready':'TRACK', 'locked':'LAND'}) smach.StateMachine.add('LAND',smach_ros.SimpleActionState('land_action', DoLandAction), {'succeeded':'succeeded', 'aborted':'aborted'}) smach_introspect = smach_ros.IntrospectionServer('commander_viewer',com,'COMMANDER') smach_introspect.start() outcome = com.execute() smach_introspect.stop() rospy.loginfo('State Machine completed with status {0}'.format(outcome))
def createSequence(self): return smach.Sequence(outcomes=[ 'TASK_COMPLETED', 'TASK_INTERRUPTED', 'TASK_FAILED', 'TASK_TIMEOUT', 'MISSION_COMPLETED' ], connector_outcome='TASK_COMPLETED')
def make_grabber(shared): sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_approach: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(APPROACH_DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add('WAIT_PIZZA', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'wreath', timeout=9999), transitions={'timeout': 'failed'}) sm_descend_grab = smach.Sequence( ['succeeded', 'failed', 'empty', 'preempted'], 'succeeded') with sm_descend_grab: smach.Sequence.add( 'CENTER_APPROACH_PIZZA', legacy_vision_states.CenterApproachObjectState(shared, 'find2_down_camera', desired_scale=90, gain=1)) smach.Sequence.add( 'ALIGN_PIZZA', legacy_vision_states.AlignObjectState(shared, 'find2_down_camera')) smach.Sequence.add( 'CENTER_APPROACH_PIZZA2', legacy_vision_states.CenterApproachObjectState(shared, 'find2_down_camera', desired_scale=120, gain=.5)) smach.Sequence.add('OPEN_GRABBER', subjugator_states.OpenGrabberState()) smach.Sequence.add( 'DOWN', common_states.WaypointState(shared, lambda cur: cur.down(1.1))) smach.Sequence.add('WAIT', common_states.SleepState(1)) smach.Sequence.add('CLOSE_GRABBER', subjugator_states.CloseGrabberState()) sm_extra_grab = smach.Sequence(['succeeded', 'empty', 'preempted'], 'succeeded') with sm_extra_grab: smach.Sequence.add('OPEN_GRABBER', subjugator_states.OpenGrabberState()) smach.Sequence.add( 'DOWN', common_states.WaypointState(shared, lambda cur: cur.down(.1))) smach.Sequence.add('WAIT', common_states.SleepState(1)) smach.Sequence.add('CLOSE_GRABBER', subjugator_states.CloseGrabberState()) sm = smach.StateMachine(['succeeded', 'failed', 'preempted']) with sm: smach.StateMachine.add('APPROACH', sm_approach, transitions={'succeeded': 'DESCEND_GRAB'}) smach.StateMachine.add('DESCEND_GRAB', sm_descend_grab, transitions={ 'succeeded': 'UP', 'failed': 'APPROACH', 'empty': 'EXTRA_GRAB' }) smach.StateMachine.add('EXTRA_GRAB', sm_extra_grab, transitions={ 'succeeded': 'UP', 'empty': 'RETRY_GRAB_COUNTER' }) smach.StateMachine.add( 'UP', common_states.WaypointState(shared, lambda cur: cur.depth(.3))) smach.StateMachine.add('RETRY_GRAB_COUNTER', common_states.CounterState(1), transitions={ 'succeeded': 'DESCEND_GRAB', 'exceeded': 'APPROACH' }) return sm
def make_shooter(shared): # Create a SMACH state machine sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_approach: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.4, 0, 0]))) smach.Sequence.add('WAIT_SHOOTER', object_finder_states.WaitForObjectsState( shared, 'find_forward', shooter_desc_cb, .95), transitions={'timeout': 'failed'}) sm_shoots = [] for color, shooter in zip(COLORS, ['left', 'right']): sm_shoot = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') sm_shoots.append(sm_shoot) with sm_shoot: smach.Sequence.add( 'APPROACH_SHOOTER', object_finder_states.ApproachObjectState(shared, 'find_forward', 'forward_camera', BOARD_DIST, marker=color)) smach.Sequence.add( 'OPEN_LOOP_FORWARD', common_states.WaypointState( shared, lambda cur: cur.forward(BOARD_DIST - HEXAGON_DIST))) smach.Sequence.add('WAIT_HEXAGON', object_finder_states.WaitForObjectsState( shared, 'find_forward', lambda: hexagon_desc_cb(SIZE, color), .99), transitions={'timeout': 'failed'}) smach.Sequence.add( 'APPROACH_HEXAGON', object_finder_states.ApproachObjectState( shared, 'find_forward', 'forward_camera', HEXAGON_DIST)) smach.Sequence.add('OPEN_LOOP_FORWARD2', common_states.WaypointState(shared, lambda cur: cur.forward(HEXAGON_DIST-SHOOT_DIST)\ .relative([0, .12, .18] if shooter == 'left' else [0, -.12, .18]))) smach.Sequence.add('SHOOT', subjugator_states.ShootTorpedoState(shooter)) sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_retreat: smach.Sequence.add( 'RETREAT', common_states.WaypointState( shared, lambda cur: cur.backward(BOARD_DIST + 1).depth(DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add('WAIT_SHOOTER', object_finder_states.WaitForObjectsState( shared, 'find_forward', shooter_desc_cb, .9), transitions={'timeout': 'failed'}) sm = smach.StateMachine(['succeeded', 'failed', 'preempted']) with sm: smach.StateMachine.add('APPROACH_SHOOT_1', sm_approach, transitions={'succeeded': 'SHOOT_1'}) smach.StateMachine.add('RETREAT_SHOOT_1', sm_retreat, transitions={'succeeded': 'SHOOT_1'}) smach.StateMachine.add('SHOOT_1', sm_shoots[0], transitions={ 'succeeded': 'RETREAT_SHOOT_2', 'failed': 'RETREAT_SHOOT_1' }) smach.StateMachine.add('RETREAT_SHOOT_2', sm_retreat, transitions={'succeeded': 'SHOOT_2'}) smach.StateMachine.add('SHOOT_2', sm_shoots[1], transitions={ 'succeeded': 'DONE_BACKUP', 'failed': 'RETREAT_SHOOT_2' }) smach.StateMachine.add('DONE_BACKUP', common_states.WaypointState( shared, lambda cur: cur.backward(.5)), transitions={'succeeded': 'succeeded'}) return sm
def main(): rospy.init_node('sekvenca1') # Create a SMACH state machine sm = smach.StateMachine(['succeeded','aborted','preempted']) # Open the container with sm: # Wake up robot smach.StateMachine.add('Wake_up', ServiceState('/pepper_robot/pose/wakeup', Empty), transitions={'succeeded':'', 'aborted':'aborted', 'preempted':'preempted'}) # Say word via tts service smach.StateMachine.add('tts', ServiceState('/tts_speech', SetString, 'First, I will show you all the poses'), transitions={'succeeded':'vocabulary', 'aborted':'aborted', 'preempted':'preempted'}) sq = smach.Sequence( outcomes = ['succeeded','aborted','preempted'], connector_outcome = 'succeeded') with sq: smach.Sequence.add('MOVE_ARM_GRAB_PRE', MoveVerticalGripperPoseActionState()) smach.Sequence.add('MOVE_GRIPPER_OPEN', MoveGripperState(GRIPPER_MAX_WIDTH)) smach.Sequence.add('MOVE_ARM_GRAB', MoveVerticalGripperPoseActionState()) smach.Sequence.add('MOVE_GRIPPER_CLOSE', MoveGripperState(grab_width)) smach.Sequence.add('MOVE_ARM_GRAB_POST', MoveVerticalGripperPoseActionState()) # sekvenca1: # tts # pokret koji je sprzen u chorographu #pokret koji je sekvenca za gripper # kombinacija sinkronizacija tts+ pokret #govor preko servisa pozvati, # Say word via tts service smach.StateMachine.add('tts', ServiceState('/tts_speech', SetString, 'First, I will show you all the poses'), transitions={'succeeded':'vocabulary', 'aborted':'aborted', 'preempted':'preempted'}) # Get desired sequence from ASR # first we need to feed vocabulari before starting asr smach.StateMachine.add('vocabulary', ServiceState('/set_vocabulary', SetString, ''), transitions={'succeeded':'asr', 'aborted':'aborted', 'preempted':'preempted'}) smach.StateMachine.add('asr', ServiceState('/start_asr', SetBool, True), transitions={'succeeded':'rest', 'aborted':'aborted', 'preempted':'preempted'}) # Go to rest pose smach.StateMachine.add('Rest', ServiceState('/pepper_robot/pose/rest', Empty), transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def main(): rospy.init_node('commander') com = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with com: smach.StateMachine.add( 'ARM', smach_ros.SimpleActionState('arm_action', DoArmAction, goal=DoArmGoal(arm_cmd=DoArmGoal.ARM)), { 'succeeded': 'WAYPOINTS', 'aborted': 'aborted' }) waypoint_sequence = smach.Sequence( outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded') with waypoint_sequence: smach.Sequence.add( 'TAKEOFF', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP0'])) smach.Sequence.add( 'WP1', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP1'])) smach.Sequence.add( 'WP2', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP2'])) smach.Sequence.add( 'WP3', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP3'])) smach.Sequence.add( 'WP4', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP2'])) smach.Sequence.add( 'WP5', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP3'])) smach.Sequence.add( 'WP6', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP1'])) smach.Sequence.add( 'WP7', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP3'])) smach.Sequence.add( 'WP8', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP2'])) smach.Sequence.add( 'WP9', smach_ros.SimpleActionState('wp_action', DoWPAction, goal=waypoints['WP1'])) smach.StateMachine.add('WAYPOINTS', waypoint_sequence, transitions={ 'succeeded': 'LAND', 'preempted': 'preempted', 'aborted': 'aborted' }) smach.StateMachine.add( 'LAND', smach_ros.SimpleActionState('land_action', DoLandAction), { 'succeeded': 'succeeded', 'aborted': 'aborted' }) smach_introspect = smach_ros.IntrospectionServer('commander_viewer', com, 'COMMANDER') smach_introspect.start() outcome = com.execute() smach_introspect.stop() rospy.loginfo('State Machine completed with status {0}'.format(outcome))
def make_hedge(shared): sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_approach: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(APPROACH_DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add('WAIT_HEDGE', legacy_vision_states.WaitForObjectsState( shared, 'find2_forward_camera', 'hedge', timeout=25), transitions={'timeout': 'failed'}) smach.Sequence.add('SLEEP', common_states.SleepState(2)) # Get a bit closer sm_center = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_center: smach.Sequence.add( 'CENTER_APPROACH_HEDGE', legacy_vision_states.CenterApproachObjectState( shared, 'find2_forward_camera', desired_scale=180)) smach.Sequence.add( 'LEFT', common_states.WaypointState(shared, lambda cur: cur.turn_left_deg(90))) smach.Sequence.add( 'GO', common_states.WaypointState(shared, lambda cur: cur.right(4))) smach.Sequence.add( 'RIGHT', common_states.WaypointState(shared, lambda cur: cur.turn_right_deg(90))) sm_pipe = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with sm_pipe: smach.Sequence.add( 'DEPTH', common_states.WaypointState( shared, lambda cur: cur.depth(constants.PIPE_DEPTH))) smach.Sequence.add( 'APPROACH', common_states.VelocityState( shared, numpy.array([constants.PIPE_SPEED, 0, 0]))) smach.Sequence.add('WAIT_PIPE', legacy_vision_states.WaitForObjectsState( shared, 'find2_down_camera', 'pipe'), transitions={'timeout': 'failed'}) smach.Sequence.add( 'STOP', common_states.WaypointState(shared, lambda cur: cur)) sm = smach.StateMachine(['succeeded', 'failed', 'preempted']) with sm: smach.StateMachine.add('APPROACH', sm_approach, transitions={ 'succeeded': 'CENTER', 'failed': 'PIPE' }) smach.StateMachine.add('CENTER', sm_center, transitions={ 'succeeded': 'PIPE', 'failed': 'PIPE' }) smach.StateMachine.add('PIPE', sm_pipe, transitions={'succeeded': 'succeeded'}) return sm
def make_manipulation(shared): sm_wheel = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'], 'succeeded') with sm_wheel: smach.Sequence.add( 'OPEN_LOOP_FORWARD', common_states.WaypointState(shared, lambda cur: cur.forward(1.9), speed=.25)) smach.Sequence.add( 'WAIT_WHEEL', legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera', 'grapes/grape', timeout=10)) smach.Sequence.add( 'APPROACH_WHEEL', legacy_vision_states.CenterObjectState(shared, 'find2_forward_camera', gain=.2)) smach.Sequence.add('EXTEND', subjugator_states.GasPoweredStickState(True)) smach.Sequence.add('OPEN_LOOP_FORWARD2', common_states.WaypointState(shared, lambda cur: cur.forward(.85)\ .relative([0, -0.02, -0.02]) .relative([0, .075, .075]), speed=.1)) smach.Sequence.add( 'TURN', common_states.WaypointSeriesState(shared, [ lambda cur: cur.left(.1), lambda cur: cur.down(.3), lambda cur: cur.right(.45), lambda cur: cur.up(.45), lambda cur: cur.left(.45), lambda cur: cur.down(.45), lambda cur: cur.right(.45) ], speed=.1)) smach.Sequence.add('RETRACT', subjugator_states.GasPoweredStickState(False)) sm_move_lever = smach.StateMachine( ['succeeded', 'timeout', 'failed', 'preempted'], input_keys=['z']) with sm_move_lever: smach.StateMachine.add('COMPARE_Z', CompareZState(), transitions=dict( higher='GO_DOWN', lower='GO_UP', )) smach.StateMachine.add( 'GO_UP', common_states.WaypointSeriesState(shared, [ lambda cur: cur.right(.08), lambda cur: cur.down(.15), lambda cur: cur.forward(1), lambda cur: cur.up(.3), lambda cur: cur.down(1), ])) smach.StateMachine.add( 'GO_DOWN', common_states.WaypointSeriesState(shared, [ lambda cur: cur.right(.08), lambda cur: cur.up(.15), lambda cur: cur.forward(1), lambda cur: cur.down(.3), lambda cur: cur.up(1), ])) sm_lever = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'], 'succeeded') with sm_lever: smach.Sequence.add( 'OPEN_LOOP_FORWARD', common_states.WaypointState(shared, lambda cur: cur.forward(1.9))) smach.Sequence.add('SAVE_Z', SaveZState()) smach.Sequence.add( 'WAIT_LEVER', legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera', 'grapes/lever', timeout=10)) smach.Sequence.add( 'APPROACH_LEVER', legacy_vision_states.CenterObjectState(shared, 'find2_forward_camera', gain=.2)) smach.Sequence.add('MOVE_LEVER', sm_move_lever) # Create a SMACH state machine sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'], 'succeeded') with sm: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(2.5))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.4, 0, 0]))) smach.Sequence.add( 'WAIT_MANIPULATION', legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera', 'grapes/board', timeout=15)) smach.Sequence.add( 'APPROACH_MANIPULATION', legacy_vision_states.CenterApproachObjectState( shared, 'find2_forward_camera', desired_scale=130e3 / 3**2)) smach.Sequence.add( 'YAW', common_states.WaypointState( shared, lambda cur: cur.heading( math.radians(-179 + (180 if cur.forward_vector[0] > 0 else 0))))) smach.Sequence.add( 'APPROACH_MANIPULATION_POST_YAW', legacy_vision_states.CenterApproachObjectState( shared, 'find2_forward_camera', desired_scale=130e3 / 3**2)) smach.Sequence.add('WHEEL', sm_wheel, transitions=dict(failed='BACKUP')) smach.Sequence.add( 'BACKUP', common_states.WaypointState(shared, lambda cur: cur.backward(3))) smach.Sequence.add( 'REFIND', common_states.VelocityState(shared, numpy.array([.1, 0, 0]))) smach.Sequence.add( 'WAIT_MANIPULATION2', legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera', 'grapes/board', timeout=15)) smach.Sequence.add( 'APPROACH_MANIPULATION2', legacy_vision_states.CenterApproachObjectState( shared, 'find2_forward_camera', desired_scale=130e3 / 3**2)) smach.Sequence.add('LEVER', sm_lever) smach.Sequence.add( 'BACKUP2', common_states.WaypointState(shared, lambda cur: cur.backward(1))) return sm
def object_gatherer_sm(target_types): """ Object gatherer SM: - explore house while looking for tables - approach each detected table and pick all the objects of the chosen types """ # look for tables state machine detect_tables_sm = smach.StateMachine( outcomes=['succeeded', 'aborted', 'preempted'], output_keys=['table', 'table_pose']) with detect_tables_sm: smach.StateMachine.add('DETECT_TABLES', MonitorTables(), transitions={ 'succeeded': 'VISITED_TABLE?', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('VISITED_TABLE?', TableWasVisited(), transitions={ 'true': 'PAUSE_DETECTION', 'false': 'succeeded' }) smach.StateMachine.add( 'PAUSE_DETECTION', Sleep(2), # improbable to see a new table for a while transitions={ 'succeeded': 'DETECT_TABLES', 'aborted': 'aborted' }) # gets called when ANY child state terminates def child_term_cb(outcome_map): # terminate all running states if FOO finished with outcome 'outcome3' if outcome_map['EXPLORE_HOUSE']: return True # terminate all running states if BAR finished if outcome_map['DETECT_TABLES']: return True # in all other case, just keep running, don't terminate anything return False # gets called when ALL child states are terminated def out_cb(outcome_map): if outcome_map['DETECT_TABLES'] == 'succeeded': return 'detected' if outcome_map['EXPLORE_HOUSE'] == 'succeeded': return 'not_detected' return outcome_map['EXPLORE_HOUSE'] # concurrence state machine: detect tables while exploring the house search_sm = smach.Concurrence( outcomes=['detected', 'not_detected', 'aborted', 'preempted'], default_outcome='not_detected', output_keys=['table', 'table_pose'], child_termination_cb=child_term_cb, outcome_cb=out_cb) with search_sm: smach.Concurrence.add('EXPLORE_HOUSE', ExploreHouse()) smach.Concurrence.add('DETECT_TABLES', detect_tables_sm) # confirm detection state machine confirm_sm = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded', input_keys=['table', 'table_pose'], output_keys=['table', 'table_pose']) with confirm_sm: smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose()) smach.Sequence.add('TURN_TO_TABLE_1', LookToPose(), remapping={'target_pose': 'table_pose'}) smach.Sequence.add('CONFIRM_TABLE_1', MonitorTables(2.0)) # 2s timeout smach.Sequence.add('TURN_TO_TABLE_2', LookToPose(), remapping={'target_pose': 'table_pose'}) smach.Sequence.add('CONFIRM_TABLE_2', MonitorTables(2.0)) # 2s timeout smach.Sequence.add('VALIDATE_SIZE', CheckTableSize()) # Full SM: explore the house and gather objects from all detected tables sm = DoOnExitContainer(outcomes=[ 'detected', 'not_detected', 'tray_full', 'aborted', 'preempted' ]) with sm: smach.StateMachine.add('SEARCH', search_sm, transitions={ 'detected': 'CONFIRM', 'not_detected': 'not_detected', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('CONFIRM', confirm_sm, transitions={ 'succeeded': 'GATHER', 'aborted': 'DISCARD', 'preempted': 'preempted' }) smach.StateMachine.add( 'DISCARD', TableMarkVisited(valid=False), # mark as invalid transitions={'succeeded': 'SEARCH'}) smach.StateMachine.add('GATHER', GatherObjects(target_types), transitions={ 'succeeded': 'SEARCH', 'aborted': 'aborted', 'preempted': 'preempted', 'tray_full': 'tray_full' }) DoOnExitContainer.add_finally( 'FOLD_ARM', FoldArm()) # fold arm on exit regardless of the outcome return sm
def GatherObjects(target_types): """ Object gatherer SM: approach the requested table and pick all the objects of the requested type """ # approach to the table approach_table_sm = smach.Sequence( outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded', input_keys=['table', 'table_pose']) with approach_table_sm: smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose()) smach.Sequence.add('CALC_APPROACHES', TableSidesPoses(cfg.APPROACH_DIST_TO_TABLE), transitions={'no_valid_table': 'aborted'}, remapping={ 'poses': 'approach_poses', 'closest_pose': 'closest_approach_pose' }) smach.Sequence.add( 'APPROACH_TABLE', GoToPose(), # use default tolerances; no precision needed here transitions={ 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'target_pose': 'closest_approach_pose'}) # detects objects over the table, and make a picking plan make_pick_plan_sm = DoOnExitContainer( outcomes=['succeeded', 'aborted', 'preempted', 'no_reachable_objs'], input_keys=['table', 'table_pose', 'object_types'], output_keys=['picking_poses', 'closest_picking_pose', 'picking_plan']) with make_pick_plan_sm: smach.StateMachine.add('GET_ROBOT_POSE', GetRobotPose(), transitions={'succeeded': 'CALC_PICK_POSES'}) smach.StateMachine.add('CALC_PICK_POSES', TableSidesPoses(cfg.PICKING_DIST_TO_TABLE), transitions={ 'succeeded': 'CALC_APPROACHES', 'no_valid_table': 'aborted' }, remapping={ 'poses': 'picking_poses', 'closest_pose': 'closest_picking_pose' }) # re-calculate approach poses when nearby for more precision smach.StateMachine.add('CALC_APPROACHES', TableSidesPoses(cfg.APPROACH_DIST_TO_TABLE), transitions={ 'succeeded': 'CALC_DETACH_POSES', 'no_valid_table': 'aborted' }, remapping={ 'poses': 'approach_poses', 'closest_pose': 'closest_approach_pose' }) smach.StateMachine.add('CALC_DETACH_POSES', TableSidesPoses(cfg.DETACH_DIST_FROM_TABLE), transitions={ 'succeeded': 'ALIGN_TO_TABLE', 'no_valid_table': 'aborted' }, remapping={ 'poses': 'detach_poses', 'closest_pose': 'closest_detach_pose' }) # from approach pose we may still misidentify some objects, so better to come as close as possible smach.StateMachine.add('ALIGN_TO_TABLE', AlignToTable(), transitions={ 'succeeded': 'DETECT_OBJECTS', 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'pose': 'closest_picking_pose'}) smach.StateMachine.add('DETECT_OBJECTS', ObjectDetection(), transitions={ 'succeeded': 'TABLE_VISITED', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('TABLE_VISITED', TableMarkVisited(), transitions={'succeeded': 'TABLE_OBSTACLE'}) smach.StateMachine.add('TABLE_OBSTACLE', TableAsObstacle(), transitions={'succeeded': 'OBJECTS_FOUND?'}, remapping={'pose': 'table_pose'}) smach.StateMachine.add('OBJECTS_FOUND?', ObjectsDetected(), transitions={ 'true': 'GROUP_OBJECTS', 'false': 'no_reachable_objs' }) smach.StateMachine.add('GROUP_OBJECTS', GroupObjects(), transitions={'succeeded': 'MAKE_PICKING_PLAN'}) smach.StateMachine.add('MAKE_PICKING_PLAN', MakePickingPlan()) DoOnExitContainer.add_finally('CLEAR_P_SCENE', ClearPlanningScene()) # go to a picking location and pick all objects reachable from there pick_objects_sm = smach.StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'tray_full'], input_keys=['table', 'picking_loc', 'object_types']) with pick_objects_sm: smach.StateMachine.add('PICK_LOC_FIELDS', PickLocFields(), transitions={'succeeded': 'GET_ROBOT_POSE'}) smach.StateMachine.add('GET_ROBOT_POSE', GetRobotPose(), transitions={'succeeded': 'AT_PICKING_POSE?'}) smach.StateMachine.add('AT_PICKING_POSE?', AreSamePose(), transitions={ 'true': 'PICK_OBJECTS', 'false': 'GOTO_APPROACH' }, remapping={ 'pose1': 'robot_pose', 'pose2': 'picking_pose' }) smach.StateMachine.add( 'GOTO_APPROACH', GoToPose(), # use default tolerances; no precision needed here transitions={'succeeded': 'ALIGN_TO_TABLE'}, remapping={'target_pose': 'approach_pose'}) smach.StateMachine.add('ALIGN_TO_TABLE', AlignToTable(), transitions={'succeeded': 'PICK_OBJECTS'}, remapping={'pose': 'picking_pose'}) smach.StateMachine.add('PICK_OBJECTS', PickReachableObjs(), transitions={'succeeded': 'CLEAR_MARKERS'}) smach.StateMachine.add('CLEAR_MARKERS', ClearMarkers(), transitions={ 'succeeded': 'DETACH_FROM_TABLE', 'aborted': 'DETACH_FROM_TABLE' }) smach.StateMachine.add('DETACH_FROM_TABLE', DetachFromTable(), remapping={'pose': 'detach_pose'}) # iterate over all picking locations in the plan pick_objects_it = smach.Iterator( outcomes=['succeeded', 'preempted', 'aborted', 'tray_full'], input_keys=['table', 'picking_plan', 'object_types'], output_keys=[], it=lambda: pick_objects_it.userdata.picking_plan, it_label='picking_loc', exhausted_outcome='succeeded') with pick_objects_it: smach.Iterator.set_contained_state('', pick_objects_sm, loop_outcomes=['succeeded']) # Full SM: approach the table, make a picking plan and execute it, double-checking that we left no object behind sm = smach.StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'tray_full'], input_keys=['table', 'table_pose']) sm.userdata.object_types = target_types with sm: smach.StateMachine.add('APPROACH_TABLE', approach_table_sm, transitions={ 'succeeded': 'RE_DETECT_TABLE', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add( 'RE_DETECT_TABLE', MonitorTables(2.0), transitions={ 'succeeded': 'MAKE_PICK_PLAN', # re-detect when nearby for more precision, 'aborted': 'succeeded' }) # or just succeed to give up if not seen again smach.StateMachine.add('MAKE_PICK_PLAN', make_pick_plan_sm, transitions={ 'succeeded': 'EXEC_PICK_PLAN', 'aborted': 'aborted', 'preempted': 'preempted', 'no_reachable_objs': 'succeeded' }) smach.StateMachine.add( 'EXEC_PICK_PLAN', pick_objects_it, transitions={ 'succeeded': 'DETECT_OBJECTS', # double-check that we left no object behind 'aborted': 'aborted', # restore original configuration on error 'preempted': 'preempted', 'tray_full': 'tray_full' }) smach.StateMachine.add('DETECT_OBJECTS', ObjectDetection(), transitions={ 'succeeded': 'OBJECTS_LEFT', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add( 'OBJECTS_LEFT', ObjectsDetected(), transitions={ 'true': 'APPROACH_TABLE', # at least one object left; restart picking 'false': 'succeeded' }) # otherwise, we are done return sm