def test_look_at_area_looks_at_correct_point(self): """Test that the robot looks at the center point of the named area, w.r.t. the frame of the entity""" entity_ds = ds.Designator(self.entity) area_ds = ds.Designator(self.area) state = states.LookAtArea(self.robot, entity_ds, area_ds, waittime=0) state.execute() vs = VectorStamped(0.5, 0.5, 0.5, "/12345") self.robot.head.look_at_point.assert_called_with(vs, timeout=0)
def __init__(self, robot, object_category_des, room_des): smach.StateMachine.__init__(self, outcomes=["done", "failed"]) room_id_des = ds.AttrDesignator(room_des, "id", resolve_type=str) with self: smach.StateMachine.add("LOOK_INTO_ROOM", NavigateToRoom(robot, room_des, room_des), transitions={ "arrived": "SAY_COME_TO_ME", "unreachable": "SAY_COME_TO_ME", "goal_not_defined": "SAY_COME_TO_ME" }) smach.StateMachine.add( "SAY_COME_TO_ME", Say(robot, "Operator, please come to me in the {room}", room=room_id_des, block=True), transitions={"spoken": "WAIT_FOR_OPERATOR"}) smach.StateMachine.add("WAIT_FOR_OPERATOR", WaitTime(4), transitions={ "waited": "ASK_WHICH_CATERGORY", "preempted": "ASK_WHICH_CATERGORY" }) smach.StateMachine.add( "ASK_WHICH_CATERGORY", AskWhichCategory( robot, ds.Designator(challenge_knowledge.category_grammar), object_category_des), transitions={"done": "done"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted']) runs = ds.Designator([0, 1]) run = ds.VariableDesignator(resolve_type=int) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'HANDLE_GUEST_1', "preempted": 'aborted', 'error': 'HANDLE_GUEST_1' }) smach.StateMachine.add('HANDLE_GUEST_1', HandleSingleGuest(robot, assume_john=True), transitions={ 'succeeded': 'HANDLE_GUEST_2', 'aborted': 'HANDLE_GUEST_2' }) smach.StateMachine.add('HANDLE_GUEST_2', HandleSingleGuest(robot, assume_john=False), transitions={ 'succeeded': 'SAY_DONE', 'aborted': 'SAY_DONE' }) smach.StateMachine.add( 'SAY_DONE', states.Say(robot, ["That's all folks, my job is done, bye bye!"], block=False), transitions={'spoken': 'GO_BACK'}) smach.StateMachine.add( 'GO_BACK', states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_door['id']), challenge_knowledge.waypoint_door['radius']), transitions={ 'arrived': 'succeeded', 'unreachable': 'succeeded', 'goal_not_defined': 'succeeded' })
def execute(self, userdata): # define answer format spec = ds.Designator("(<positive_answer>|<negative_answer>)") # define choices choices = ds.Designator({ "positive_answer": ["Yes", "Correct", "Right", "Yup"], "negative_answer": ["No", "Incorrect", "Wrong", "Nope"]}) answer = ds.VariableDesignator(resolve_type=GetSpeechResponse) state = HearOptionsExtra(self.robot, spec, choices, answer.writeable) # execute listen outcome = state.execute() if not outcome == "heard": # if there was no answer print "HearYesNo: did not hear anything!" return 'heard_failed' else: response_negative = "" response_positive = "" # test if the answer was positive, if its empty it will return excepton and continue to negative answer try: response_positive = answer.resolve().choices["positive_answer"] print "HearYesNo: answer is positive, heard: '" + response_positive + "'" return 'heard_yes' except KeyError, ke: print "KeyError resolving the answer heard: " + str(ke) pass try: response_negative = answer.resolve().choices["negative_answer"] print "HearYesNo: answer is negative, heard: '" + response_negative + "'" return 'heard_no' except KeyError, ke: print "KeyError resolving the answer heard: " + str(ke) pass
def test_look_on_top_of_entity_looks_at_correct_point(self): """Test that the robot looks at the center point of the named area, w.r.t. the frame of the entity""" entity_ds = ds.Designator(self.entity) state = states.LookOnTopOfEntity(self.robot, entity_ds, waittime=0) state.execute() vs = VectorStamped( 0, 0, 1, # This is the height of the object, as indicated by the z_max "/12345") # The ID self.robot.head.look_at_point.assert_called_with(vs)
def execute(self, userdata=None): limit_reached = 0 for i in range(self._nr_tries): rospy.loginfo("AskPersonName") self.robot.speech.speak("What is your name?", block=True) names_spec = "T['name': N] -> NAME[N]\n\n" for dn in self.name_options: names_spec += "NAME['{name}'] -> {name}\n".format(name=dn) spec = ds.Designator(names_spec) answer = ds.VariableDesignator(resolve_type=HMIResult) state = HearOptionsExtra(self.robot, spec, answer.writeable) try: outcome = state.execute() if not outcome == "heard": limit_reached += 1 if limit_reached == self._nr_tries: self.person_name_des.write(self.default_name) rospy.logwarn( "Speech recognition outcome was not successful. Using default name '{}'" .format(self.person_name_des.resolve())) return 'failed' if outcome == "heard": try: rospy.logdebug("Answer: '{}'".format(answer.resolve())) name = answer.resolve().semantics["name"] rospy.loginfo( "This person's name is: '{}'".format(name)) self.person_name_des.write(str(name)) rospy.loginfo( "Result received from speech recognition is '" + name + "'") except KeyError as ke: rospy.loginfo("KeyError resolving the name heard: " + str(ke)) return 'failed' return 'succeeded' except TimeoutException: return "timeout" return 'succeeded'
parser = argparse.ArgumentParser( description= "Place an imaginary object at provided x, y, z coordinates using the " "'Place' smach state") parser.add_argument("x", type=float, help="x-coordinate (in map)") parser.add_argument("y", type=float, help="y-coordinate (in map)") parser.add_argument("z", type=float, help="z-coordinate (in map)") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_placing") robot = get_robot(args.robot) pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") item = Entity("dummy_id", "dummy_type", None, None, None, None, None, None) arm = ds.ArmDesignator(robot, arm_properties={ "required_trajectories": ["prepare_place"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) place_state = Place(robot, ds.Designator(item), ds.Designator(pose), arm) place_state.execute()
def __init__(self, robot, door_waypoint, guest_ent_des, guest_name_des, guest_drink_des): """ Learn what a guest looks like and what his/her favourite drink is :param robot: Robot that should execute this state :param door_waypoint: Entity-designator resolving to a waypoint Where are guests expected to come in :param guest_ent_des: Entity of the guest :param guest_name_des: designator that the name (str) of the guest is written to :param guest_drink_des: designator that the drink type (str) of the drink the guest wants """ smach.StateMachine.__init__( self, outcomes=['succeeded', 'failed', 'aborted']) self.drink_spec_des = ds.Designator( challenge_knowledge.common.drink_spec, name='drink_spec') with self: smach.StateMachine.add( 'GOTO_DOOR', states.NavigateToWaypoint( robot, door_waypoint, challenge_knowledge.waypoint_door['radius']), transitions={ 'arrived': 'SAY_OPEN_DOOR', 'unreachable': 'SAY_OPEN_DOOR', 'goal_not_defined': 'aborted' }) smach.StateMachine.add( 'SAY_OPEN_DOOR', states.Say( robot, ["Someone please open the door, I'm expecting guests"], block=True, look_at_standing_person=True), transitions={'spoken': 'SAY_PLEASE_COME_IN'}) smach.StateMachine.add( 'SAY_PLEASE_COME_IN', states.Say(robot, [ "Please come in, I'm waiting for someone to step in front of me" ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_GUEST'}) smach.StateMachine.add("WAIT_FOR_GUEST", states.WaitForPersonInFront( robot, attempts=30, sleep_interval=1), transitions={ 'success': 'SAY_HELLO', 'failed': 'SAY_PLEASE_COME_IN' }) smach.StateMachine.add( 'SAY_HELLO', states.Say(robot, ["Hi there, I'll learn your face now"], block=True, look_at_standing_person=True), transitions={'spoken': 'ASK_GUEST_NAME'}) smach.StateMachine.add('ASK_GUEST_NAME', states.AskPersonName( robot, guest_name_des.writeable, challenge_knowledge.common.names), transitions={ 'succeeded': 'LEARN_PERSON', 'failed': 'SAY_HELLO', 'timeout': 'SAY_HELLO' }) smach.StateMachine.add('LEARN_PERSON', states.LearnPerson( robot, name_designator=guest_name_des), transitions={ 'succeeded': 'SAY_GUEST_LEARNED', 'failed': 'SAY_FAILED_LEARNING' }) smach.StateMachine.add( 'SAY_FAILED_LEARNING', states.Say(robot, ["Not sure if I remember you, but I'll do my best"], block=False), transitions={'spoken': 'SAY_DRINK_QUESTION'}) smach.StateMachine.add( 'SAY_GUEST_LEARNED', states.Say(robot, ["Okidoki, now I know what you look like"], block=True), transitions={'spoken': 'SAY_DRINK_QUESTION'}) smach.StateMachine.add('SAY_DRINK_QUESTION', states.Say(robot, ["What's your favorite drink?"], block=True), transitions={'spoken': 'HEAR_DRINK_ANSWER'}) smach.StateMachine.add('HEAR_DRINK_ANSWER', states.HearOptionsExtra( robot, self.drink_spec_des, guest_drink_des.writeable), transitions={ 'heard': 'RESET_1', 'no_result': 'SAY_DRINK_QUESTION' }) smach.StateMachine.add('RESET_1', states.ResetArms(robot), transitions={'done': 'succeeded'})
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"})
from geometry_msgs.msg import Pose if len(sys.argv) < 2: print("Please provide robot name as argument.") sys.exit(1) robot_name = sys.argv[1] rospy.init_node('test_take_orders') robot = get_robot(robot_name) pose = Pose() pose.position.x = 1.0 pose.position.y = 1.0 pose.position.z = 1.6 customer_entity = Entity( 'random_id', 'person', '/map', # FrameID can only be map frame unfortunately, Our KDL wrapper doesn't do well with PoseStampeds etc. 'dummy', # Only pose and frame_id are used 'shape', 'volumes', 'super_types', 'last_update_time') customer_entity.pose = pose # This takes care of the conversion to KDL for us orders = [] sm = TakeOrder(robot, ds.Designator(customer_entity), orders=orders) sm.execute() rospy.loginfo("Orders {}".format(orders))
def test_collection_non_iterable(self): self.assertRaises(AssertionError, IterateDesignator, ds.Designator('a'), ds.VariableDesignator(resolve_type=str).writeable)
def __init__(self, robot, seats_to_inspect, room, seat_is_for=None): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) seats = SeatsInRoomDesignator(robot, seats_to_inspect, room, "seats_in_room") seat_ent_des = ds.VariableDesignator(resolve_type=Entity) if seat_is_for: ds.check_type(seat_is_for, str) else: seat_is_for = ds.Designator(' ') with self: smach.StateMachine.add( 'SAY_LETS_FIND_SEAT', states.SayFormatted(robot, [ "Let me find a place for {name} to sit. Please be patient while I check out where there's place to sit" ], name=seat_is_for, block=False), transitions={'spoken': 'ITERATE_NEXT_SEAT'}) smach.StateMachine.add('ITERATE_NEXT_SEAT', states.IterateDesignator( seats, seat_ent_des.writeable), transitions={ 'next': 'CHECK_SEAT_EMPTY', 'stop_iteration': 'SAY_NO_EMPTY_SEATS' }) smach.StateMachine.add('CHECK_SEAT_EMPTY', states.CheckVolumeEmpty( robot, seat_ent_des, 'on_top_of', 0.2), transitions={ 'occupied': 'ITERATE_NEXT_SEAT', 'empty': 'POINT_AT_EMPTY_SEAT', 'partially_occupied': 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', 'failed': 'ITERATE_NEXT_SEAT' }) smach.StateMachine.add( 'POINT_AT_EMPTY_SEAT', states.PointAt(robot=robot, arm_designator=ds.UnoccupiedArmDesignator( robot, {'required_goals': ['point_at']}), point_at_designator=seat_ent_des, look_at_designator=seat_ent_des), transitions={ "succeeded": "SAY_SEAT_EMPTY", "failed": "SAY_SEAT_EMPTY" }) smach.StateMachine.add( 'SAY_SEAT_EMPTY', states.SayFormatted(robot, ["Please sit on the {seat}, {name}"], name=seat_is_for, seat=ds.AttrDesignator(seat_ent_des, 'id', resolve_type=str), block=True), transitions={'spoken': 'RESET_SUCCESS'}) smach.StateMachine.add( 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', states.PointAt(robot=robot, arm_designator=ds.UnoccupiedArmDesignator( robot, {'required_goals': ['point_at']}), point_at_designator=seat_ent_des, look_at_designator=seat_ent_des), transitions={ "succeeded": "SAY_SEAT_PARTIALLY_OCCUPIED", "failed": "SAY_SEAT_PARTIALLY_OCCUPIED" }) smach.StateMachine.add( 'SAY_SEAT_PARTIALLY_OCCUPIED', states.SayFormatted(robot, [ "I think there's some space left here where you can sit {name}" ], name=seat_is_for, block=True), transitions={'spoken': 'RESET_SUCCESS'}) smach.StateMachine.add( 'SAY_NO_EMPTY_SEATS', states.SayFormatted(robot, [ "Sorry, there are no empty seats. I guess you just have to stand {name}" ], name=seat_is_for, block=True), transitions={'spoken': 'RESET_FAIL'}) smach.StateMachine.add('RESET_FAIL', states.ResetArms(robot), transitions={'done': 'failed'}) smach.StateMachine.add('RESET_SUCCESS', states.ResetArms(robot), transitions={'done': 'succeeded'})
robot = get_robot(args.robot) entity_id = "test_item" pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") robot.ed.update_entity(id=entity_id, frame_stamped=pose) shape = RightPrism([ kdl.Vector(0, 0, 0), kdl.Vector(0, 0.05, 0), kdl.Vector(0.05, 0.05, 0), kdl.Vector(0.05, 0, 0) ], -0.1, 0.1) item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape, None, None, None) item = ds.Designator(item) arm = ds.UnoccupiedArmDesignator(robot, arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) grab_state = Grab(robot, item, arm) grab_state.execute()
def single_item(robot, results_writer, cls, support, waypoint, inspect_from_area=None, non_strict_class=False, search_area='on_top_of'): """ Benchmark grasping for a single item. Outputs a record dictionary :param robot: an instance of Robot :param results_writer: a csv.DictWriter to which the output record is written :param cls: class/type of item to grab :param support: ID of the entity supporting the item-to-grab :param waypoint: From where should the robot start the grasp :param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection :param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object :param search_area: which area of the support-entity to search/inspect for an item of the given class :return: a dict with the benchmark result """ grasp_cls = ds.Designator(cls, name='grasp_cls') support_entity = ds.EdEntityDesignator(robot, id=support, name='support_entity') entity_ids = ds.VariableDesignator([], resolve_type=[ClassificationResult], name='entity_ids') waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint') arm = ds.LockingDesignator( ds.UnoccupiedArmDesignator(robot, name='unoccupied-arm', arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] })) arm.lock() record = { 'robot': robot.robot_name, 'start_waypoint': waypoint, 'expected_class': cls, 'id': None, 'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') } try: say_announce = Say( robot, sentence="Please put a {cls} {search_area} the {support}".format( cls=cls, support=support, search_area=search_area)) say_announce.execute() nav_to_start = NavigateToWaypoint(robot, waypoint_designator=waypoint_des, look_at_designator=support_entity) assert nav_to_start.execute( ) == 'arrived', "I did not arrive at the start" inspect = Inspect(robot=robot, entityDes=support_entity, objectIDsDes=entity_ids, navigation_area=inspect_from_area, searchArea=search_area) record['inspect_start'] = time.time() assert inspect.execute( ) == 'done', "I could not inspect the support entity" record['inspect_end'] = time.time() record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] - record['inspect_start']) inspection_result = entity_ids.resolve( ) # type: List[ClassificationResult] if inspection_result: if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class: rospy.loginfo("Any item will match") matching_results = inspection_result else: matching_results = [ result for result in inspection_result if result.type == grasp_cls.resolve() ] rospy.loginfo("Found {} items of class {}".format( len(matching_results), grasp_cls.resolve())) if matching_results: if len(matching_results) > 1: rospy.logwarn( "There are multiple items OK to grab, will select the last one" ) record['observed_class'] = ' '.join( [result.type for result in matching_results]) else: record['observed_class'] = matching_results[-1].type selected_entity_id = matching_results[-1].id rospy.loginfo("Selected entity {} for grasping".format( selected_entity_id)) grasp_entity = ds.EdEntityDesignator(robot, id=selected_entity_id) record['id'] = selected_entity_id[:6] entity = grasp_entity.resolve() # type: Entity if entity: vector_stamped = entity.pose.extractVectorStamped( ) # type: VectorStamped record['x'] = '{:.3f}'.format(vector_stamped.vector.x()) record['y'] = '{:.3f}'.format(vector_stamped.vector.y()) record['z'] = '{:.3f}'.format(vector_stamped.vector.z()) grab_state = Grab(robot, grasp_entity, arm) record['grab_start'] = time.time() assert grab_state.execute() == 'done', "I couldn't grasp" record['grab_end'] = time.time() record['grab_duration'] = '{:3.3f}'.format( record['grab_end'] - record['grab_start']) assert nav_to_start.execute( ) == 'arrived', "I could not navigate back to the start" rospy.logwarn("Robot will turn around to drop the {}".format( grasp_cls.resolve())) force_drive = ForceDrive(robot, 0, 0, 1, 3.14) # rotate 180 degs in pi seconds force_drive.execute() say_drop = Say( robot, sentence="I'm going to drop the item, please hold it!") say_drop.execute() drop_it = SetGripper(robot, arm_designator=arm, grab_entity_designator=grasp_entity) drop_it.execute() force_drive_back = ForceDrive( robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds force_drive_back.execute() nav_to_start.execute() else: raise AssertionError("No {} found".format(grasp_cls.resolve())) else: rospy.logerr("No entities found at all :-(") except AssertionError as assertion_err: say_fail = Say(robot, sentence=assertion_err.message + ", sorry") say_fail.execute() finally: results_writer.writerow(record) return record
import rospy from robot_smach_states.designator_iterator import IterateDesignator import robot_smach_states.util.designators as ds if __name__ == "__main__": collection = ['a', 'b', 'c'] collection_des = ds.Designator(collection) element_des = ds.VariableDesignator(resolve_type=str) iterator = IterateDesignator(collection_des, element_des.writeable) for i in range(8): rospy.loginfo("iterator.execute() \n {}".format(iterator.execute())) rospy.loginfo("element_des.resolve() \n {} \n".format(element_des.resolve()))
def setUpClass(cls): cls.collection_des = ds.Designator(['a', 'b', 'c']) cls.element_des = ds.VariableDesignator(resolve_type=str).writeable cls.iterator = IterateDesignator(cls.collection_des, cls.element_des)
def __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: self.spec_des = ds.Designator(knowledge.location_grammar) self.answer_des = ds.VariableDesignator(resolve_type=HMIResult) self.entity_des = EntityFromHmiResults(robot, self.answer_des) self._location_hmi_attempt = 0 self._max_hmi_attempts = 3 # ToDo: parameterize? @smach.cb_interface(outcomes=["reset"]) def _reset_location_hmi_attempt(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ self._location_hmi_attempt = 0 return "reset" smach.StateMachine.add("RESET_HMI_ATTEMPT", smach.CBState(_reset_location_hmi_attempt), transitions={"reset": "ANNOUNCE_ITEM"}) if WAIT_MODE == WaitMode.SPEECH: smach.StateMachine.add( "ANNOUNCE_ITEM", Say(robot, "Hello, my name is {}. Please call me by my name. " "Talk loudly into my microphone and wait for the ping". format(robot.robot_name), block=True), transitions={"spoken": "WAIT_TO_BE_CALLED"}) smach.StateMachine.add( "WAIT_TO_BE_CALLED", HearOptions(robot, ["{}".format(robot.robot_name)], timeout=10), transitions={ "{}".format(robot.robot_name): "INSTRUCT", "no_result": "ANNOUNCE_ITEM" }) elif WAIT_MODE == WaitMode.VISUAL: smach.StateMachine.add( "ANNOUNCE_ITEM", Say(robot, "Hello, my name is {}. Please step in front of me.". format(robot.robot_name), block=True), transitions={"spoken": "WAIT_TO_BE_CALLED"}) smach.StateMachine.add("WAIT_TO_BE_CALLED", WaitForPersonInFront( robot, attempts=10, sleep_interval=1.0), transitions={ "success": "INSTRUCT", "failed": "SAY_NOT_DETECTED" }) smach.StateMachine.add( "SAY_NOT_DETECTED", Say(robot, "I did not see you but will try to continue anyway.". format(robot.robot_name), block=True), transitions={"spoken": "INSTRUCT"}) smach.StateMachine.add( "INSTRUCT", Say(robot, [ "Please tell me where you would like to go. " "Talk loudly into my microphone and wait for the ping" ], block=True), transitions={"spoken": "LISTEN_FOR_LOCATION"}) smach.StateMachine.add("LISTEN_FOR_LOCATION", HearOptionsExtra(robot, self.spec_des, self.answer_des.writeable, 6), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "HANDLE_FAILED_HMI" }) smach.StateMachine.add( "ASK_CONFIRMATION", Say(robot, [ "I hear that you would like to go to the {place}, is this correct?" ], place=ds.AttrDesignator(self.entity_des, "id", resolve_type=str)), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", HearOptions(robot=robot, options=["yes", "no"]), transitions={ "yes": "INSTRUCT_FOR_WAIT", "no": "INSTRUCT", "no_result": "HANDLE_FAILED_HMI" }) @smach.cb_interface(outcomes=["retry", "fallback", "failed"]) def _handle_failed_hmi(userdata=None): """ Handle failed HMI queries so we can try up to x times """ self._location_hmi_attempt += 1 # Increment if self._location_hmi_attempt == self._max_hmi_attempts: rospy.logwarn( "HMI failed for the {} time, returning 'failed'". format(self._max_hmi_attempts)) if not BACKUP_SCENARIOS: rospy.logwarn( "No fallback scenario's available anymore") return "failed" backup = BACKUP_SCENARIOS.pop(0) robot.speech.speak("I am sorry but I did not hear you", mood="sad", block=False) robot.speech.speak(backup.sentence, block=False) self.answer_des.writeable.write( HMIResult("", backup.entity_id)) return "fallback" rospy.loginfo( "HMI failed for the {} time out of {}, retrying".format( self._location_hmi_attempt, self._max_hmi_attempts)) return "retry" smach.StateMachine.add("HANDLE_FAILED_HMI", smach.CBState(_handle_failed_hmi), transitions={ "retry": "INSTRUCT", "fallback": "INSTRUCT_FOR_WAIT", "failed": "failed" }) smach.StateMachine.add( "INSTRUCT_FOR_WAIT", Say(robot, [ "Let me think how to get to the {entity_id}", "I will now determine the best route to the {entity_id}" ], entity_id=ds.AttrDesignator(self.entity_des, "id", resolve_type=str)), transitions={"spoken": "GIVE_DIRECTIONS"}) smach.StateMachine.add("GIVE_DIRECTIONS", GiveDirections(robot, self.entity_des), transitions={ "succeeded": "INSTRUCT_FOLLOW", "failed": "failed" }) smach.StateMachine.add("INSTRUCT_FOLLOW", Say(robot, ["Please follow me"], block=True), transitions={"spoken": "GUIDE_OPERATOR"}) smach.StateMachine.add("GUIDE_OPERATOR", GuideToRoomOrObject(robot, self.entity_des), transitions={ "arrived": "SUCCESS", "unreachable": "SAY_CANNOT_REACH", "goal_not_defined": "SAY_CANNOT_REACH", "lost_operator": "SAY_LOST_OPERATOR", "preempted": "failed" }) smach.StateMachine.add( "SUCCESS", Say(robot, ["We have arrived"], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add( "SAY_CANNOT_REACH", Say(robot, ["I am sorry but I cannot reach the destination."], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add( "SAY_LOST_OPERATOR", Say(robot, ["Oops I have lost you completely."], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add("RETURN_TO_INFORMATION_POINT", NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, INFORMATION_POINT_ID)), transitions={ "arrived": "succeeded", "unreachable": "failed", "goal_not_defined": "failed" })
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) # Designators # Room to search through roomr = ds.VariableDesignator(resolve_type=str) roomw = roomr.writeable # Cleanup location as defined in local knowledge cleanup_locationsr = ds.VariableDesignator([{'1': '2', '3': '4'}]) cleanup_locationsw = cleanup_locationsr.writeable location_des = ds.VariableDesignator(resolve_type=dict) with sm: smach.StateMachine.add("START_ROBUST", robot_smach_states.StartChallengeRobust( robot, challenge_knowledge.starting_point), transitions={ "Done": "GO_TO_WAITING_POINT", "Aborted": "GO_TO_WAITING_POINT", "Failed": "GO_TO_WAITING_POINT" }) smach.StateMachine.add("GO_TO_WAITING_POINT", robot_smach_states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, challenge_knowledge.waiting_point)), transitions={ "arrived": "INQUIRE_ROOM", "unreachable": "GO_TO_WAITING_POINT1", "goal_not_defined": "GO_TO_WAITING_POINT1" }) smach.StateMachine.add("GO_TO_WAITING_POINT1", robot_smach_states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, challenge_knowledge.waiting_point), radius=0.3), transitions={ "arrived": "INQUIRE_ROOM", "unreachable": "INQUIRE_ROOM", "goal_not_defined": "INQUIRE_ROOM" }) smach.StateMachine.add("INQUIRE_ROOM", AskWhichRoomToClean( robot, ds.Designator(challenge_knowledge.grammar), roomw, cleanup_locationsw), transitions={"done": "VERIFY"}) smach.StateMachine.add('VERIFY', VerifyWorldModelInfo(robot), transitions={ "done": "SAY_START_CHALLENGE", "failed": "SAY_KNOWLEDGE_NOT_COMPLETE" }) smach.StateMachine.add( 'SAY_KNOWLEDGE_NOT_COMPLETE', robot_smach_states.Say(robot, [ "My knowledge of the world is not complete!", "Please give me some more information!" ], block=False), transitions={"spoken": "Aborted"}) smach.StateMachine.add('SAY_START_CHALLENGE', robot_smach_states.Say(robot, [ "Starting the cleanup challenge", "What a mess here, let's clean this room!", "Let's see if I can find some garbage here", "All I want to do is clean this mess up!" ], block=False), transitions={"spoken": "ITERATE_NEXT_LOC"}) smach.StateMachine.add('ITERATE_NEXT_LOC', robot_smach_states.IterateDesignator( cleanup_locationsr, location_des.writeable), transitions={ "next": "INSPECT", "stop_iteration": "RETURN_TO_OPERATOR" }) smach.StateMachine.add("INSPECT", CleanInspect(robot, location_des), transitions={"done": "ITERATE_NEXT_LOC"}) smach.StateMachine.add("RETURN_TO_OPERATOR", robot_smach_states.NavigateToWaypoint( robot=robot, waypoint_designator=ds.EntityByIdDesignator( robot=robot, id=challenge_knowledge.starting_point), radius=0.3), transitions={ "arrived": "SAY_CLEANED_ROOM", "unreachable": "SAY_CLEANED_ROOM", "goal_not_defined": "SAY_CLEANED_ROOM" }) smach.StateMachine.add( 'SAY_CLEANED_ROOM', robot_smach_states.Say(robot, [ "I successfully cleaned the {room}!", "All done in the {room}. Am I a good robot now?", "There, I cleaned up your mess in the {room}, are you happy now!" ], room=roomr, block=False), transitions={"spoken": "Done"}) return sm