def __init__(self, robot, door_start_wp_designator, door_dest_wp_designator, approach_speed=0.1, push_speed=0.05, attempts=10): """ Push against a door until its open :param robot: Robot on which to execute this state machine :param door_entity_designator: The door entity. Defaults to None, which implies the door its in front of This entity must have 2 fields in its data: push_start_waypoint and push_destination_waypoint, which must both contain an ID of a waypoint. push_start_waypoint is from where the robot will start pushing push_destination_waypoint simply needs to be reachable for the door the be considered 'open' :param approach_speed: Speed with which to approach the door :param push_speed: Speed with which to push against the door :return: """ smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) #door_start_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='start', name='door_open_start') #door_dest_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='destination', name='door_open_dest') with self: smach.StateMachine.add( 'GOTO_DOOR_START', states.NavigateToWaypoint(robot, door_start_wp_designator, radius=0.05), transitions={'arrived' :'PUSH_DOOR_ITERATOR', 'unreachable' :'failed', 'goal_not_defined' :'failed'}) # START REPEAT DOOR OPENING push_door_iterator = smach.Iterator(outcomes=['open', 'closed', 'failed'], it = lambda:range(0, attempts), it_label='counter', input_keys=[], output_keys=[], exhausted_outcome = 'failed') with push_door_iterator: push_door = smach.StateMachine( outcomes = ['open', 'closed', 'failed']) with push_door: smach.StateMachine.add( 'APPROACH_AGAINST_N', ForceDriveToTouchDoor(robot, approach_speed), transitions={'front':'closed', 'left':'CHECK_DOOR_PASSABLE', 'right':'CHECK_DOOR_PASSABLE', 'failed':'failed'}) smach.StateMachine.add( 'CHECK_DOOR_PASSABLE', CheckDoorPassable(robot, destination_designator=door_dest_wp_designator), transitions={'blocked':'closed', 'passable':'open'}) smach.Iterator.set_contained_state( 'APPROACH_AGAINST', push_door, loop_outcomes=['failed', 'closed'], break_outcomes=['open']) smach.StateMachine.add( 'PUSH_DOOR_ITERATOR', push_door_iterator, { 'open':'succeeded', 'closed':'failed', 'failed':'failed'})
def searchRoutine(yaw, initialAngle, finalAngle, left=True, center=False): searchSpan = finalAngle - initialAngle direction = searchSpan > 0 searchSpan = abs(searchSpan) numIt = ceil(searchSpan / 20) search = smach.Iterator(outcomes=['success', 'abort'], input_keys=[], output_keys=['outAngle'], it=lambda: range(0, int(numIt)), it_label='index', exhausted_outcome='abort') search.userdata.initialAngle = initialAngle with search: container_sm = smach.StateMachine( outcomes=['success', 'abort', 'continue'], output_keys=['outAngle']) container_sm.userdata.timeout = 500 container_sm.userdata.angle = 20 container_sm.userdata.initialAngle = initialAngle container_sm.userdata.outAngle = 0 with container_sm: smach.StateMachine.add('InitialRotate', RotateTo(yaw, increment=True, direction=1), transitions={ 'success': 'Localize', 'abort': 'abort' }, remapping={ 'timeout': 'timeout', 'angle': 'initialAngle' }) smach.StateMachine.add('Rotate', RotateTo(yaw, increment=True, direction=direction), transitions={ 'success': 'continue', 'abort': 'abort' }, remapping={ 'timeout': 'timeout', 'angle': 'angle' }) smach.StateMachine.add('Localize', Localize(True, left, center=center), transitions={ 'success': 'success', 'failure': 'Rotate' }, remapping={'angle': 'outAngle'}) smach.Iterator.set_contained_state('SEARCH', container_sm, loop_outcomes=['continue']) return search
def main(): rospy.init_node('smach_example_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) #sm.userdata.num = 5 sm.userdata.result = [] # Open the container with sm: # Add states to the container smach.StateMachine.add('FOO', Foo(), transitions={ 'succeeded': 'SUB', 'aborted': 'aborted', 'preempted': 'preempted' }) sm_ite = smach.Iterator( outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['result'], output_keys=['result'], #it = lambda: range(sm.userdata.num), #it = lambda: ['i', 'my', 'me'], #it = ['i', 'my', 'me'], #it = range(5), #it = dynamic_iterator, it=static_list, it_label='it_data', exhausted_outcome='succeeded') with sm_ite: # if BAR's result is 'succeeded' then loop, otherwise exit with result. smach.Iterator.set_contained_state('BAR', Bar(), loop_outcomes=['succeeded']) smach.StateMachine.add('SUB', sm_ite, transitions={ 'succeeded': 'BAS', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.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 PlaceObject(attempts=3): """ Place a given object, retrying up to a given number of times """ it = smach.Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_name', 'support_surf', 'place_pose'], output_keys=[], it=lambda: range(0, attempts), it_label='attempt', exhausted_outcome='aborted') with it: sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted', 'continue'], input_keys=['object_name', 'support_surf', 'place_pose'], output_keys=[]) with sm: smach.StateMachine.add( 'PlaceObject', smach_ros.SimpleActionState( 'place_object', thorp_msgs.PlaceObjectAction, goal_slots=['object_name', 'support_surf', 'place_pose'], result_slots=[]), remapping={ 'object_name': 'object_name', 'support_surf': 'support_surf', 'place_pose': 'place_pose' }, transitions={ 'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'ClearOctomap' }) smach.StateMachine.add('ClearOctomap', smach_ros.ServiceState( 'clear_octomap', std_srvs.Empty), transitions={ 'succeeded': 'continue', 'preempted': 'preempted', 'aborted': 'aborted' }) # TODOs: # - we should open the gripper, in case we have picked an object # - check error and, if collision between parts of the arm, move a bit the arm --> not enough info # - this doesn't make too much sense as a loop... better try all our tricks and exit # - can I reuse the same for place and MoveToTarget??? smach.Iterator.set_contained_state('', sm, loop_outcomes=['continue']) return it
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") pdf_writer = WritePdf(robot=robot) with self: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SAY_UNABLE_TO_OPEN_DOOR', 'abort': 'Aborted' }) smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR', states.Say( robot, "I am unable to open the shelf door, " "can you please open it for me?"), transitions={'spoken': 'AWAIT_START'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None, manipulate_machine=None): """ Moves the entities for this challenge to the correct poses""" # Determine where to perform the challenge robot_pose = robot.base.get_location() ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm()) cabinet_id = ENTITY_POSES[0][2] table_id = ENTITY_POSES[0][3] # Update the world model robot.ed.update_entity(id="balcony_shelf", frame_stamped=FrameStamped( kdl.Frame(kdl.Rotation(), kdl.Vector(0.0, 3.0, 0.0)), frame_id="map")) robot.ed.update_entity(id=cabinet_id, frame_stamped=ENTITY_POSES[0][0]) robot.ed.update_entity(id=table_id, frame_stamped=ENTITY_POSES[0][1]) # Update designators cabinet.id_ = ENTITY_POSES[0][2] room.id_ = ENTITY_POSES[0][4] # Update manipulate machine manipulate_machine.place_entity_designator.id_ = cabinet_id manipulate_machine.place_designator._area = ENTITY_POSES[0][5] manipulate_machine.place_designator.place_location_designator.id = cabinet_id manipulate_machine.table_designator.id_ = table_id manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2] return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table, cb_args=[single_item]), transitions={'done': 'NAV_TO_START'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'INSPECT_SHELVES', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, cabinet), transitions={ 'succeeded': 'WRITE_PDF_SHELVES', 'nothing_found': 'WRITE_PDF_SHELVES', 'failed': 'WRITE_PDF_SHELVES' }) smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded') with range_iterator: smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "manipulation")
def __init__(self, robot, personNameDesLocal): @smach.cb_interface(outcomes=['spoken']) def sayIsYourName(userdata=None): printOk("sayIsYourName") robot.speech.speak("I heard " + personNameDes.resolve() + ". Is this correct?", block=True) return 'spoken' @smach.cb_interface(outcomes=['spoken']) def sayCouldNotLearnNameCB(userdata=None): printOk("sayCouldNotLearnNameCB") robot.speech.speak( "Sorry but I could not understand your name. I will just call you " + personNameDesLocal.resolve(), block=False) return 'spoken' smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: learnNameIterator = smach.Iterator( outcomes=['iterator_success', 'iterator_failed'], it=lambda: range(0, 3), it_label='counter', input_keys=[], output_keys=[], exhausted_outcome='iterator_failed') with learnNameIterator: # for some reason the container needs to have the same outcomes as the iterator, plus a "continue" state learnNameContainer = smach.StateMachine(outcomes=[ 'iterator_success', 'iterator_failed', 'inner_container_failed' ]) with learnNameContainer: smach.StateMachine.add('ASK_PERSON_NAME', test_states.AskPersonName( robot, personNameDesLocal.writeable), transitions={ 'succeeded': 'SAY_IS_YOUR_NAME', 'failed': 'SAY_HEAR_FAILED' }) smach.StateMachine.add( 'SAY_IS_YOUR_NAME', smach.CBState(sayIsYourName), transitions={'spoken': 'HEAR_YES_NO_1'}) smach.StateMachine.add('HEAR_YES_NO_1', states_interaction.HearYesNo(robot), transitions={ 'heard_yes': 'iterator_success', 'heard_no': 'SAY_NAME_WRONG', 'heard_failed': 'SAY_HEAR_FAILED' }) smach.StateMachine.add( 'SAY_NAME_WRONG', states.Say( robot, ["Sorry, I understood wrong.", "Oh I'm sorry."], block=False), transitions={'spoken': 'inner_container_failed'}) smach.StateMachine.add( 'SAY_HEAR_FAILED', states.Say(robot, [ "I did not understand your name.", "I didn't listen correctly." ], block=False), transitions={'spoken': 'inner_container_failed'}) smach.Iterator.set_contained_state( 'LEARN_NAME_CONTAINER', learnNameContainer, loop_outcomes=['inner_container_failed']) # break_outcomes=['iterator_success']) smach.StateMachine.add('LEARN_NAME_ITERATOR', learnNameIterator, transitions={ 'iterator_success': 'container_success', 'iterator_failed': 'SAY_COULD_NOT_LEARN_NAME' }) smach.StateMachine.add('SAY_COULD_NOT_LEARN_NAME', smach.CBState(sayCouldNotLearnNameCB), transitions={'spoken': 'container_failed'})
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 __init__(self, robot): smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"]) hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult) information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator( hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str) information_point_designator = ds.EdEntityDesignator( robot, id_designator=information_point_id_designator) with self: single_item = InformMachine(robot) if START_ROBUST: smach.StateMachine.add("START_CHALLENGE", states.StartChallengeRobust( robot, INITIAL_POSE_ID), transitions={ "Done": "ASK_WHERE_TO_GO", "Aborted": "Aborted", "Failed": "Aborted" }) smach.StateMachine.add( "ASK_WHERE_TO_GO", states.Say( robot, "Near which furniture object should I go to start guiding people?" ), transitions={"spoken": "WAIT_WHERE_TO_GO"}) smach.StateMachine.add( "WAIT_WHERE_TO_GO", states.HearOptionsExtra( robot=robot, spec_designator=ds.Designator( initial_value=START_GRAMMAR), speech_result_designator=hmi_result_des.writeable), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "ASK_WHERE_TO_GO" }) # ToDo: add fallbacks #option: STORE_STARTING_POSE smach.StateMachine.add( "ASK_CONFIRMATION", states.Say(robot, [ "I hear that you would like me to start the tours at" " the {place}, is this correct?" ], place=information_point_id_designator, block=True), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", states.HearOptions( robot=robot, options=["yes", "no"]), transitions={ "yes": "MOVE_OUT_OF_MY_WAY", "no": "ASK_WHERE_TO_GO", "no_result": "ASK_WHERE_TO_GO" }) smach.StateMachine.add( "MOVE_OUT_OF_MY_WAY", states.Say(robot, "Please move your ass so I can get going!"), transitions={"spoken": "TC_MOVE_TIME"}) smach.StateMachine.add("TC_MOVE_TIME", states.WaitTime(robot=robot, waittime=3), transitions={ "waited": "NAV_TO_START", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START", states.NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "in_front_of" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "WAIT_NAV_BACKUP", "goal_not_defined": "Aborted" }) # If this happens: never mind smach.StateMachine.add("WAIT_NAV_BACKUP", states.WaitTime(robot, 3.0), transitions={ "waited": "NAV_TO_START_BACKUP", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START_BACKUP", states.NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "near" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "SAY_CANNOT_REACH_WAYPOINT", # Current pose backup "goal_not_defined": "Aborted" }) # If this happens: never mind @smach.cb_interface(outcomes=["done"]) def _turn_around(userdata=None): """ Turns the robot approximately 180 degrees around """ v_th = 0.5 robot.base.force_drive(vx=0.0, vy=0.0, vth=v_th, timeout=math.pi / v_th) return "done" smach.StateMachine.add( "TURN_AROUND", smach.CBState(_turn_around), transitions={"done": "STORE_STARTING_POSE"}) smach.StateMachine.add( "SAY_CANNOT_REACH_WAYPOINT", states.Say( robot, "I am not able to reach the starting point." "I'll use this as starting point"), transitions={"spoken": "STORE_STARTING_POSE"}) else: smach.StateMachine.add("INITIALIZE", states.Initialize(robot), transitions={ "initialized": "STORE_STARTING_POSE", "abort": "Aborted" }) ## This is purely for a back up scenario until the range iterator @smach.cb_interface(outcomes=["succeeded"]) def store_pose(userdata=None): base_loc = robot.base.get_location() base_pose = base_loc.frame location_id = INFORMATION_POINT_ID robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped( base_pose, "/map"), type="waypoint") return "succeeded" smach.StateMachine.add("STORE_STARTING_POSE", smach.CBState(store_pose), transitions={"succeeded": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator( outcomes=["succeeded", "failed"], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(1000), it_label="index", exhausted_outcome="succeeded") with range_iterator: smach.Iterator.set_contained_state( "SINGLE_ITEM", single_item, loop_outcomes=["succeeded", "failed"]) smach.StateMachine.add("RANGE_ITERATOR", range_iterator, { "succeeded": "AT_END", "failed": "Aborted" }) # End setup iterator smach.StateMachine.add("AT_END", states.Say(robot, "Goodbye"), transitions={"spoken": "Done"})
def construct_sm(): broadcaster = TFBroadCaster() tf_cast_loop = rospy.Timer(rospy.Duration(0.1), broadcaster.loop) sm = smach.StateMachine(outcomes=['success', 'failure']) sm.userdata.nums = range(5) sm.userdata.tf_x = 0 sm.userdata.tf_y = 0 sm.userdata.tf_z = 0 move_base_client = actionlib.SimpleActionClient('/move_base', MoveBaseAction) rospy.loginfo('wait for move_base action server') move_base_client.wait_for_server() with sm: @smach.cb_interface(outcomes=['success']) def start_cb(ud): rospy.loginfo('start the test') move_arm_neutral() return 'success' smach.StateMachine.add('START', smach.CBState(start_cb), transitions={'success': 'ITERATOR'}) iterator = smach.Iterator(outcomes=['success', 'failure'], input_keys=['tf_x', 'tf_y', 'tf_z'], output_keys=[], it=lambda: range(0, len(sm.userdata.nums)), it_label='index', exhausted_outcome='success') with iterator: container_sm = smach.StateMachine( input_keys=['tf_x', 'tf_y', 'tf_z', 'index'], outcomes=['continue', 'failure']) with container_sm: ### PUT OBJECT IN THE SHELF @smach.cb_interface(outcomes=['success']) def put_object_cb(ud): delete_object('apple') x = round(np.random.uniform(2.0, 2.4), 1) put_object('apple', x, 4.75, 0.5) return 'success' smach.StateMachine.add('PUTOBJECT', smach.CBState(put_object_cb), transitions={'success': 'NAVOBSTACLE'}) ### MOVE TO THE CENTER OF THE SHELF smach.StateMachine.add('NAVOBSTACLE', UpdateObstacle(0.5), transitions={'success': 'Move2Shelf'}) goal = MoveBaseGoal() goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = 2.2 goal.target_pose.pose.position.y = 4.1 goal.target_pose.pose.position.z = 0 quaternion = tf.transformations.quaternion_from_euler( 0, 0, 1.57) goal.target_pose.pose.orientation.x = quaternion[0] goal.target_pose.pose.orientation.y = quaternion[1] goal.target_pose.pose.orientation.z = quaternion[2] goal.target_pose.pose.orientation.w = quaternion[3] smach.StateMachine.add('Move2Shelf', SimpleActionState('/move_base', MoveBaseAction, goal=goal), transitions={ 'succeeded': 'FindApple', 'preempted': 'failure', 'aborted': 'failure' }) ### FIND THE APPLE smach.StateMachine.add('FindApple', FindApple(), transitions={'success': 'BROADCASTTF'}) ### REGISTER AND BROADCAST THE TF @smach.cb_interface(input_keys=['tf_x', 'tf_y', 'tf_z'], outcomes=['success']) def broadcast_tf_cb(ud): while not rospy.is_shutdown(): result = move_arm_neutral() if result is True: break #broadcast head_rgbd_sensor to apple position broadcaster.add(ud.tf_x, ud.tf_y, ud.tf_z, 0.5, 0, 0, 'apple', 'head_rgbd_sensor_rgb_frame') approach_x = ud.tf_x tate = 0.1 * math.tan(0.5) approach_y = ud.tf_y + tate approach_z = ud.tf_z - 0.1 broadcaster.add(approach_x, approach_y, approach_z, 0.5, 0, 0, 'approach_p', 'head_rgbd_sensor_rgb_frame') hand_to_map = get_relative_coordinate('map', 'approach_p') broadcaster.add(hand_to_map.translation.x, hand_to_map.translation.y, hand_to_map.translation.z, -1.57, -1.57, 0, 'map_approach_p', 'map') return 'success' smach.StateMachine.add( 'BROADCASTTF', smach.CBState(broadcast_tf_cb), transitions={'success': 'GRASPOBSTACLE'}) ### UPDATE OBSTACLE RECONFIGURE FOR AVOID HAND OBSTACLE smach.StateMachine.add('GRASPOBSTACLE', UpdateObstacle(0), transitions={'success': 'MOVE2CENTER'}) @smach.cb_interface(outcomes=['success']) def move_to_center_cb(ud): while not rospy.is_shutdown(): relative_p = get_relative_coordinate2( "hand_palm_link", "map_approach_p") duration = (rospy.Time.now() - relative_p.header.stamp).to_sec() if duration < 1: break goal = MoveBaseGoal() goal.target_pose.header.frame_id = "base_footprint" goal.target_pose.pose.position.x = 0 goal.target_pose.pose.position.y = -relative_p.transform.translation.y goal.target_pose.pose.orientation = quaternion_from_euler( 0, 0, 0) navclient.send_goal(goal) navclient.wait_for_result() state = navclient.get_state() return 'success' ### MOVE TO THE CENTER OF THE OBJECT smach.StateMachine.add('MOVE2CENTER', smach.CBState(move_to_center_cb), transitions={'success': 'GRASPOBJECT'}) ### GRASP THE OBJECT @smach.cb_interface(outcomes=['success']) def grasp_object_cb(ud): add_collision_scene(frame_id='map_approach_p') result = move_hand(1) while not rospy.is_shutdown(): # trans = get_relative_coordinate("map", "approach_p") trans = get_relative_coordinate( "map", "map_approach_p") result = move_wholebody_ik(trans.translation.x, trans.translation.y, trans.translation.z, -90, 0, -90, 'map') if result == True: break goal = MoveBaseGoal() goal.target_pose.header.frame_id = "base_footprint" goal.target_pose.pose.position.x = 0.15 goal.target_pose.pose.position.y = 0 goal.target_pose.pose.position.z = 0 goal.target_pose.pose.orientation = quaternion_from_euler( 0, 0, 0) # goal.target_pose.header.frame_id = "hand_palm_link" # goal.target_pose.pose.position.x = 0.15 # goal.target_pose.pose.position.y = 0 # goal.target_pose.pose.position.z = 0 # goal.target_pose.pose.orientation = quaternion_from_euler(0, 0, 0) navclient.send_goal(goal) navclient.wait_for_result() state = navclient.get_state() move_hand(0) move_base_vel(-1, 0, 0) remove_collision_scene() while not rospy.is_shutdown(): result = move_arm_init() if result is True: break broadcaster.clear() return 'success' smach.StateMachine.add('GRASPOBJECT', smach.CBState(grasp_object_cb), transitions={'success': 'CHECKGRASP'}) ### CHECK GRASP THE OBJECT OR NOT smach.StateMachine.add('CHECKGRASP', CheckGrasp(), transitions={ 'success': 'ORIGINOBSTACLE', 'failure': 'Move2Shelf' }) ### UPDATE OBSTACLE FOR THE NAVIGATION smach.StateMachine.add('ORIGINOBSTACLE', UpdateObstacle(0.5), transitions={'success': 'Move2Origin'}) ### MOVE TO THE ORIGIN 0,0,0 IN MAP REFERENCE goal = MoveBaseGoal() goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = 0 goal.target_pose.pose.position.y = 0 goal.target_pose.pose.position.z = 0 quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) goal.target_pose.pose.orientation.x = quaternion[0] goal.target_pose.pose.orientation.y = quaternion[1] goal.target_pose.pose.orientation.z = quaternion[2] goal.target_pose.pose.orientation.w = quaternion[3] smach.StateMachine.add('Move2Origin', SimpleActionState('/move_base', MoveBaseAction, goal=goal), transitions={ 'succeeded': 'OPENGRIPPER', 'preempted': 'failure', 'aborted': 'failure' }) ### OPENGRIPPER @smach.cb_interface(outcomes=['success']) def open_gripper_cb(ud): result = move_hand(1) return 'success' smach.StateMachine.add('OPENGRIPPER', smach.CBState(open_gripper_cb), transitions={'success': 'DELETEOBJECT'}) ### DETELE OBJECT READY FOR THE NEXT TASK @smach.cb_interface(outcomes=['success']) def delete_object_cb(ud): delete_object('apple') return 'success' smach.StateMachine.add('DELETEOBJECT', smach.CBState(delete_object_cb), transitions={'success': 'continue'}) smach.Iterator.set_contained_state( 'CONTAINER_STATE', container_sm, loop_outcomes=['continue', 'failure']) smach.StateMachine.add('ITERATOR', iterator, transitions={ 'success': 'success', 'failure': 'failure' }) return sm
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") placed_items = [] with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add("INIT_WM", InitializeWorldModel(robot), transitions={'done': 'AWAIT_START'}) # smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR", # states.Say(robot, ["Hi there, I will now wait until you remove the cup", # "I'm waiting for you to remove the cup"], block=False), # transitions={"spoken": "WAIT_FOR_DOOR"}) # # smach.StateMachine.add("WAIT_FOR_DOOR", # states.WaitForDoorOpen(robot, timeout=10), # transitions={"closed": "DOOR_CLOSED", # "open": "AWAIT_START"}) # # smach.StateMachine.add("DOOR_CLOSED", # states.Say(robot, ["I am waiting for you to remove the cup", # "I'd start, if you remove the cup from my laser"]), # transitions={"spoken": "WAIT_FOR_DOOR"}) if USE_SLAM: drive_state = "RESET_ED_SLAM" else: drive_state = "NAV_TO_START" smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': drive_state, 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) if USE_SLAM: # vth = 1.0 # smach.StateMachine.add("NAV_TO_FIT_POSE", # ForceDrive(robot, 0, 0, vth, 3.14/vth), # transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("RESET_ED_SLAM", states.ResetED(robot), transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("FIT_ENTITY", FitEntity(robot, CABINET), transitions={ 'succeeded': 'NAV_TO_START', 'failed': 'SAY_FITTING_FAILED' }) smach.StateMachine.add( "SAY_FITTING_FAILED", states.Say(robot, [ "Fitting the {0} failed, I will stop now.".format(CABINET) ], mood="sad"), transitions={'spoken': 'Aborted'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'FORCE_ROTATE', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("FORCE_ROTATE", ForceRotate(robot, 0.5, 2.0, 30.0), transitions={ 'done': "NAV_TO_START", 'timedout': "INSPECT_SHELVES" }) # smach.StateMachine.add("RESET_ED", # states.ResetED(robot), # transitions={'done' :'INSPECT_SHELVES'}) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, OBJECT_SHELVES), transitions={ 'succeeded': 'EXPORT_PDF', 'nothing_found': 'EXPORT_PDF', 'failed': 'EXPORT_PDF' }) @smach.cb_interface(outcomes=["exported"]) def export_to_pdf(userdata=None): global DETECTED_OBJECTS_WITH_PROBS entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS] # Export images (Only best MAX_NUM_ENTITIES_IN_PDF) # pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge") return "exported" smach.StateMachine.add('EXPORT_PDF', smach.CBState(export_to_pdf), transitions={'exported': 'RANGE_ITERATOR'}) # Begin setup iterator range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], #Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded' ) #The exhausted argument should be set to the preffered state machine outcome with range_iterator: single_item = ManipRecogSingleItem( robot, ds.VariableDesignator(placed_items, [Entity], name="placed_items")) smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(sm, "manipulation") return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") pdf_writer = WritePdf(robot=robot) with self: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SAY_UNABLE_TO_OPEN_DOOR', 'abort': 'Aborted' }) smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR', states.Say( robot, "I am unable to open the shelf door, " "can you please open it for me?"), transitions={'spoken': 'AWAIT_START'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None, manipulate_machine=None): """ Moves the entities for this challenge to the correct poses""" # Determine where to perform the challenge # Apparently, the indices here come from: # (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area) robot_pose = robot.base.get_location() WORKSPACES.sort( key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p - robot_pose.frame.p).Norm()) closest_workspace = WORKSPACES[0] rospy.loginfo( "Closest workspace: grasp from '{grasp}' and place on '{place}'" .format( grasp=closest_workspace.grasp_entity_conf.entity_id, place=closest_workspace.place_entity_conf.entity_id)) cabinet_id = closest_workspace.place_entity_conf.entity_id table_id = closest_workspace.grasp_entity_conf.entity_id # Update the world model by fitting the entities to the frame_stamped's given below. robot.ed.update_entity(id=cabinet_id, frame_stamped=closest_workspace. place_entity_conf.pose_estimate) robot.ed.update_entity(id=table_id, frame_stamped=closest_workspace. grasp_entity_conf.pose_estimate) # Update designators cabinet.id_ = closest_workspace.place_entity_conf.entity_id # Update manipulate machine manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[ 0] manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table, cb_args=[single_item]), transitions={'done': 'NAV_TO_START'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'INSPECT_SHELVES', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, cabinet), transitions={ 'succeeded': 'WRITE_PDF_SHELVES', 'nothing_found': 'WRITE_PDF_SHELVES', 'failed': 'WRITE_PDF_SHELVES' }) smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded') with range_iterator: smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "manipulation")
def __init__(self): super(ExploreHouse, self).__init__(outcomes=['succeeded', 'aborted', 'preempted']) # explore a single room explore_1_room_sm = DoOnExitContainer( outcomes=['succeeded', 'aborted', 'preempted'], input_keys=[ 'map_image', 'map_resolution', 'map_origin', 'robot_radius', 'segmented_map', 'room_number', 'room_information_in_meter', 'completed_rooms' ], output_keys=['completed_rooms']) with explore_1_room_sm: smach.StateMachine.add( 'HAVE_EXPLORE_PLAN?', UDIfKey('coverage_path'), # already have a explore plan? transitions={ 'true': 'INSERT_CURRENT_POSE', # then skip to traverse poses 'false': 'GET_ROBOT_POSE' }) smach.StateMachine.add('GET_ROBOT_POSE', GetRobotPose(), transitions={ 'succeeded': 'PLAN_ROOM_EXPL', 'aborted': 'aborted' }) smach.StateMachine.add('PLAN_ROOM_EXPL', PlanRoomExploration(), transitions={ 'succeeded': 'GOTO_START_POSE', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add( 'GOTO_START_POSE', GoToPose( dist_tolerance=cfg.LOOSE_DIST_TOLERANCE, # close enough angle_tolerance=cfg.INF_ANGLE_TOLERANCE), # ignore yaw transitions={ 'succeeded': 'INSERT_START_POSE', 'preempted': 'preempted', 'aborted': 'INSERT_CURRENT_POSE' }, remapping={'target_pose': 'start_pose'}) smach.StateMachine.add('INSERT_START_POSE', UDInsertInList(0), transitions={ 'succeeded': 'TRAVERSE_POSES', 'aborted': 'aborted' }, remapping={ 'element': 'start_pose', 'list': 'coverage_path_pose_stamped' }) smach.StateMachine.add( 'INSERT_CURRENT_POSE', PrependCurrentPose(), # otherwise we can retake the remaining transitions={ 'succeeded': 'TRAVERSE_POSES', # plan wherever it is closer from us, 'aborted': 'aborted' }, # instead of from the beginning remapping={'path': 'coverage_path_pose_stamped'}) smach.StateMachine.add( 'TRAVERSE_POSES', FollowWaypoints(), transitions={ 'succeeded': 'ROOM_COMPLETED', 'preempted': 'DEL_TRAVERSED_WPS', 'aborted': 'aborted' }, remapping={'waypoints': 'coverage_path_pose_stamped'}) smach.StateMachine.add( 'DEL_TRAVERSED_WPS', DelTraversedWPs(), # deleted the traversed waypoints so we can transitions={'succeeded': 'preempted' }, # resume exploration from where we left it remapping={'waypoints': 'coverage_path_pose_stamped'}) smach.StateMachine.add('ROOM_COMPLETED', RoomCompleted(), transitions={'succeeded': 'succeeded'}) DoOnExitContainer.add_finally( 'CLEAR_EXPLORE_PLAN', UDSetToNone('coverage_path'), # we only keep current run_on=['succeeded', 'aborted']) # plan when preempted (so we can resume) # 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', 'room_sequence' ], output_keys=['completed_rooms'], it=self.get_room_sequence, 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']) with self: smach.StateMachine.add( 'HAVE_SEGMENTED_MAP?', UDHasKey('segmented_map'), # already have a rooms map? transitions={ 'true': 'HAVE_ROOM_SEQUENCE?', # use it for planning! 'false': 'SEGMENT_ROOMS' }) smach.StateMachine.add('SEGMENT_ROOMS', SegmentRooms(), transitions={ 'succeeded': 'GET_ROBOT_POSE', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add( 'HAVE_ROOM_SEQUENCE?', UDHasKey('room_sequence'), # already have a plan? transitions={ 'true': 'EXPLORE_HOUSE', # follow it! 'false': 'GET_ROBOT_POSE' }) smach.StateMachine.add('GET_ROBOT_POSE', GetRobotPose(), transitions={ 'succeeded': 'PLAN_ROOM_SEQ', 'aborted': 'aborted' }) smach.StateMachine.add('PLAN_ROOM_SEQ', PlanRoomSequence(), transitions={ 'succeeded': 'EXPLORE_HOUSE', 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'input_map': 'map_image'}) smach.StateMachine.add('EXPLORE_HOUSE', explore_house_it, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' })
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
def bumpBuoy(surge, sway, yaw, timeout): bump = smach.StateMachine(outcomes=['success', 'abort']) numIt = 3 with bump: bump.userdata.back = 180 bump.userdata.speed = 0.25 bump.userdata.backTime = 20 bump.userdata.zero = 0 bump.userdata.bumpTime = 10 it = smach.Iterator(outcomes=['success', 'abort'], input_keys=[], output_keys=[], it=lambda: range(0, int(numIt)), it_label='index', exhausted_outcome='success') with it: container_sm = smach.StateMachine( outcomes=['success', 'abort', 'continue']) container_sm.userdata.moveTime = 6 container_sm.userdata.zero = 0 container_sm.userdata.speed = 0.3 container_sm.userdata.angle = None container_sm.userdata.timeout = timeout with container_sm: smach.StateMachine.add('Move', Move(surge, sway), transitions={'success': 'ReLocalize'}, remapping={ 'angle': 'zero', 'speed': 'speed', 'moveTime': 'moveTime' }) smach.StateMachine.add('ReLocalize', Localize(False, True), transitions={ 'success': 'RotateTo', 'failure': 'abort' }, remapping={'outAngle': 'angle'}) smach.StateMachine.add('RotateTo', RotateTo(yaw), transitions={ 'success': 'continue', 'abort': 'abort' }, remapping={ 'timeout': 'timeout', 'angle': 'angle' }) smach.Iterator.set_contained_state("BumpIt", container_sm, loop_outcomes=['continue']) smach.StateMachine.add("BUMP_IT", it, transitions={ 'success': 'FinalBump', 'abort': 'abort' }) smach.StateMachine.add("FinalBump", Move(surge, sway), transitions={'success': 'MoveBack'}, remapping={ 'angle': 'zero', 'speed': 'speed', 'moveTime': 'bumpTime' }) smach.StateMachine.add("MoveBack", Move(surge, sway), transitions={'success': 'success'}, remapping={ 'angle': 'back', 'speed': 'speed', 'moveTime': 'backTime' }) return bump
def main(): rospy.init_node('example_action_smach_state_machine') #TEST CONCURRENCE STATES WITH ACTIONS-------------------------------------------------------------------------------------------------- test_con1 = smach.Concurrence(outcomes=['test_done'], default_outcome = 'test_done', input_keys = ['test_order'], output_keys= ['test_output_order'], child_termination_cb = lambda outcome_map : True, outcome_cb = lambda outcome_map : 'test_done') # Test Concurrence with goal and result callbacks that grab data from # the userdata input key passed into the state machine with test_con1: def test_con1_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence)) userdata.test_output_order = 6 return 'succeeded' def test_con1_goal_cb(userdata, goal): test_goal = ExampleGoal() test_goal.order = userdata.test_order return test_goal smach.Concurrence.add('TestCon1', SimpleActionState('example_server', ExampleAction, input_keys=['test_order'], output_keys=['test_output_order'], goal_cb=test_con1_goal_cb, result_cb=test_con1_result_cb)) smach.Concurrence.add('testTimeout',sleep(20)) test_con2 = smach.Concurrence(outcomes=['test_done'], default_outcome = 'test_done', input_keys = ['test_order'], child_termination_cb = lambda outcome_map : True, outcome_cb = lambda outcome_map : 'test_done') with test_con2: def test_con2_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence)) return 'succeeded' def test_con2_goal_cb(userdata, goal): test_goal = ExampleGoal() test_goal.order = userdata.test_order return test_goal smach.Concurrence.add('TestCon2', SimpleActionState('example_server', ExampleAction, input_keys=['test_order'], goal_cb=test_con2_goal_cb, result_cb=test_con2_result_cb)) smach.Concurrence.add('testTimeout',sleep(20)) test_con3 = smach.Concurrence(outcomes=['test_done'], default_outcome = 'test_done', child_termination_cb = lambda outcome_map : True, outcome_cb = lambda outcome_map : 'test_done') with test_con3: def test_con3_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence)) return 'succeeded' test_goal = ExampleGoal() test_goal.order = 7 smach.Concurrence.add('TestCon3', SimpleActionState('example_server', ExampleAction, goal=test_goal, result_cb=test_con3_result_cb)) smach.Concurrence.add('testTimeout',sleep(20)) #SETUP ITERATOR STATE MACHINE-------------------------------------------------------------------------- sm_it = smach.StateMachine(outcomes=['outcome6']) sm_it.userdata.nums = range(25, 88, 3) sm_it.userdata.even_nums = [] sm_it.userdata.odd_nums = [] with sm_it: tutorial_it = smach.Iterator(outcomes = ['succeeded','aborted'], input_keys = ['nums', 'even_nums', 'odd_nums'], it = lambda: range(0, len(sm_it.userdata.nums)), output_keys = ['even_nums', 'odd_nums'], it_label = 'index', exhausted_outcome = 'succeeded') with tutorial_it: container_sm = smach.StateMachine(outcomes = ['succeeded','preempted','aborted','continue'], input_keys = ['nums', 'index', 'even_nums', 'odd_nums'], output_keys = ['even_nums', 'odd_nums']) with container_sm: #test wether even or odd with a conditional state smach.StateMachine.add('EVEN_OR_ODD', smach_ros.ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, input_keys=['nums', 'index']), {'true':'ODD', 'false':'EVEN' }) #add even state using a callback state @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def even_cb(ud): rospy.sleep(1) ud.even_nums.append(ud.nums[ud.index]) return 'succeeded' smach.StateMachine.add('EVEN', smach.CBState(even_cb), {'succeeded':'continue'}) #add odd state using a callback state @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def odd_cb(ud): rospy.sleep(1) ud.odd_nums.append(ud.nums[ud.index]) return 'succeeded' smach.StateMachine.add('ODD', smach.CBState(odd_cb), {'succeeded':'continue'}) #close container_sm smach.Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['continue']) smach.StateMachine.add('TUTORIAL_IT',tutorial_it, {'succeeded':'outcome6', 'aborted':'outcome6'}) #STATE MACHINE----------------------------------------------------------------------------------------- # Create a SMACH state machine sm_top = smach.StateMachine(outcomes=['outcome5']) # Create userdata for top level state machine sm_top.userdata.first_goal = 5 sm_top.userdata.pass_data = 0 # Open the top container with sm_top: # Add Concurrent States smach.StateMachine.add('TestCon1', test_con1, transitions={'test_done':'TestCon2'}, remapping={'test_order':'first_goal', 'test_output_order':'pass_data'}) smach.StateMachine.add('TestCon2', test_con2, transitions={'test_done':'TestCon3'}, remapping={'test_order':'pass_data'}) smach.StateMachine.add('TestCon3', test_con3, transitions={'test_done':'SUB'}) # Create and add a low tier statemachine sm_sub = smach.StateMachine(outcomes=['outcome4']) with sm_sub: smach.StateMachine.add('FOO', example_states.Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'}) smach.StateMachine.add('BAR', example_states.Bar(), transitions={'outcome2':'FOO'}) # Add the low tier statemachine smach.StateMachine.add('SUB', sm_sub, transitions={'outcome4':'TUT_IT'}) # Add the low tier statemachine smach.StateMachine.add('TUT_IT', sm_it, transitions={'outcome6':'TestCon1'}) # Execute SMACH plan sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT') sis.start() outcome = sm_top.execute() rospy.spin() sis.stop()
def __init__(self): super(PickReachableObjs, self).__init__( outcomes=['succeeded', 'aborted', 'preempted', 'tray_full'], input_keys=['object_types']) # pick a single object sm pick_1_obj_sm = smach.StateMachine( outcomes=[ 'continue', 'succeeded', 'aborted', 'preempted', 'tray_full' ], input_keys=['object_types', 'failures']) pick_1_obj_sm.userdata.max_effort = cfg.GRIPPER_MAX_EFFORT with pick_1_obj_sm: smach.StateMachine.add('DETECT_OBJECTS', ObjectDetection(), transitions={ 'succeeded': 'SELECT_TARGET', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('SELECT_TARGET', TargetSelection(), transitions={ 'have_target': 'PICKUP_OBJECT', 'no_targets': 'succeeded' }, remapping={'target': 'object'}) smach.StateMachine.add('PICKUP_OBJECT', PickupObject(), transitions={ 'succeeded': 'PLACE_ON_TRAY', 'preempted': 'preempted', 'aborted': 'RECORD_FAILURE' }) smach.StateMachine.add('PLACE_ON_TRAY', PlaceInTray(), transitions={ 'succeeded': 'CLEAR_FAILURES', 'preempted': 'preempted', 'aborted': 'CLEAR_GRIPPER', 'tray_full': 'tray_full' }) smach.StateMachine.add('CLEAR_GRIPPER', ClearGripper(), transitions={ 'succeeded': 'RECORD_FAILURE', 'preempted': 'aborted', 'aborted': 'aborted' }) smach.StateMachine.add('RECORD_FAILURE', RecordFailure(), transitions={ 'succeeded': 'continue', 'max_failures': 'aborted' }) smach.StateMachine.add('CLEAR_FAILURES', ClearFailures(), transitions={'succeeded': 'continue'}) pick_reach_objs_it = smach.Iterator( outcomes=['succeeded', 'preempted', 'aborted', 'tray_full'], input_keys=['object_types', 'failures'], output_keys=[], it=range(25), # kind of while true it_label='iteration', exhausted_outcome='succeeded') with pick_reach_objs_it: smach.Iterator.set_contained_state('', pick_1_obj_sm, loop_outcomes=['continue']) self.userdata.failures = { } # keep track of pick failures for each object with self: smach.StateMachine.add('PICKUP_OBJECTS', pick_reach_objs_it, transitions={ 'succeeded': 'FOLD_ARM', 'preempted': 'preempted', 'aborted': 'aborted' }) smach.StateMachine.add('FOLD_ARM', FoldArm(), transitions={ 'succeeded': 'DETECT_OBJECTS', 'preempted': 'preempted', 'aborted': 'aborted' }) smach.StateMachine.add('DETECT_OBJECTS', ObjectDetection(), transitions={ 'succeeded': 'SELECT_TARGET', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add( 'SELECT_TARGET', TargetSelection( ), # just used to check if there are objects left transitions={ 'have_target': 'PICKUP_OBJECTS', # restart picking if so 'no_targets': 'succeeded' }) DoOnExitContainer.add_finally('CLEAR_P_SCENE', ClearPlanningScene()) DoOnExitContainer.add_finally('FINAL_FOLD_ARM', FoldArm())
smach.StateMachine.add('GetDetectedCubes', GetDetectedCubes(), remapping={'object_names': 'object_names'}, transitions={ 'succeeded': 'StackCubes', 'retry': 'ObjectDetection' }) # Stack cubes sub state machine; iterates over the detected cubes and stack them over the one most in front of the arm sc_it = smach.Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[ 'base_cube_pose', 'base_cube_name', 'other_cubes', 'cube_height', 'support_surf' ], output_keys=[], it=lambda: sm.userdata. other_cubes, # must be a lambda because we destroy the list it_label='object_name', exhausted_outcome='succeeded') with sc_it: sc_sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted', 'continue'], input_keys=[ 'base_cube_pose', 'object_name', 'support_surf', 'cube_height' ], output_keys=[]) with sc_sm: smach.StateMachine.add('PickupObject', PickupObject(),
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") pdf_writer = WritePdf(robot=robot) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'AWAIT_START', 'abort': 'Aborted'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START'}) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None): """ 'Locks' a locking designator """ # For now, don't do anything return "done" # Move away the cabinet robot.ed.update_entity(id="cabinet", frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(12.0, 0, 0)), frame_id="map")) # Determine where to perform the challenge robot_pose = robot.base.get_location() ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm()) # Update the world model robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0]) robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1]) return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table), transitions={'done': 'NAV_TO_START'}) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic(robot, {cabinet: "in_front_of"}, cabinet), transitions={'arrived': 'INSPECT_SHELVES', 'unreachable': 'INSPECT_SHELVES', 'goal_not_defined': 'INSPECT_SHELVES'}) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot), transitions={'succeeded': 'WRITE_PDF_SHELVES', 'nothing_found': 'WRITE_PDF_SHELVES', 'failed': 'WRITE_PDF_SHELVES'}) smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator(outcomes=['succeeded', 'failed'], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded') with range_iterator: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff smach.Iterator.set_contained_state('SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {'succeeded': 'AT_END', 'failed': 'Aborted'}) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "manipulation")