From 1efdb8065eecf427d48be9ce5317df885afb28a1 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Sat, 16 Jun 2018 20:31:03 -0400 Subject: [PATCH 01/71] Fix dependencies --- action_server/package.xml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/action_server/package.xml b/action_server/package.xml index 61b11e11..75b2307d 100644 --- a/action_server/package.xml +++ b/action_server/package.xml @@ -10,9 +10,10 @@ catkin - action_server_msgs - grammar_parser - robot_skills - actionlib - + action_server_msgs + grammar_parser + robot_skills + actionlib + challenge_presentation + challenge_presentation From 8bc7dbd02209b9e710ff3d8b357e768e6cc83936 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 11:26:40 -0400 Subject: [PATCH 02/71] feat(count_and_tell): dummy action for counting objects and reporting --- .../action_server/actions/count_and_tell.py | 72 +++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 action_server/src/action_server/actions/count_and_tell.py diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py new file mode 100644 index 00000000..2deffe50 --- /dev/null +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -0,0 +1,72 @@ +from action import Action, ConfigurationData +from entity_description import resolve_entity_description + +import rospy + + +class CountAndTell(Action): + ''' The CountAndTell class implements the action to count the number of objects and tell the operator. + + Parameters to pass to the configure() method are: + - `sentence` (required): The sentence to speak. May be a keyword to tell something more intelligent. + ''' + def __init__(self): + Action.__init__(self) + self._required_skills = ['speech'] + + class Semantics: + def __init__(self): + self.location = None + self.object = None + + @staticmethod + def _parse_semantics(semantics_dict): + semantics = CountAndTell.Semantics() + + semantics.location = resolve_entity_description(semantics_dict['location']) + semantics.object = resolve_entity_description(semantics_dict['object']) + + return semantics + + def _configure(self, robot, config): + self._robot = robot + + semantics = CountAndTell._parse_semantics(config.semantics) + + self._sentence = "I understand you want me to count the number of {} on the {} and tell you. " \ + "But I can't do that yet. Sorry. Next task please.".format(semantics.object.type, + semantics.location.id) + + self._config_result.succeeded = True + return + + def _start(self): + self._robot.speech.speak(self._sentence, block=True) + self._execute_result.succeeded = True + + def _cancel(self): + pass + + +if __name__ == "__main__": + rospy.init_node('say_test') + + import sys + robot_name = sys.argv[1] + if robot_name == 'amigo': + from robot_skills.amigo import Amigo as Robot + elif robot_name == 'sergio': + from robot_skills.sergio import Sergio as Robot + else: + from robot_skills.mockbot import Mockbot as Robot + + robot = Robot() + + action = CountAndTell() + + config = ConfigurationData({'action': 'count-and-tell', + 'location': {'id': 'cabinet'}, + 'object': {'type': 'beer'}}) + + action.configure(robot, config) + action.start() From 8634596af392f4208841a23b28299a41dedd3271 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 11:26:55 -0400 Subject: [PATCH 03/71] feat(count_and_tell): dummy action for asking name and reporting --- .../actions/tell_name_of_person.py | 69 +++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 action_server/src/action_server/actions/tell_name_of_person.py diff --git a/action_server/src/action_server/actions/tell_name_of_person.py b/action_server/src/action_server/actions/tell_name_of_person.py new file mode 100644 index 00000000..eaeb7ea4 --- /dev/null +++ b/action_server/src/action_server/actions/tell_name_of_person.py @@ -0,0 +1,69 @@ +from action import Action, ConfigurationData +from entity_description import resolve_entity_description + +import rospy + + +class TellNameOfPerson(Action): + ''' The TellNameOfPerson class implements the action to ask someones name and report it to the operator. + + Parameters to pass to the configure() method are: + - `sentence` (required): The sentence to speak. May be a keyword to tell something more intelligent. + ''' + def __init__(self): + Action.__init__(self) + self._required_skills = ['speech'] + + class Semantics: + def __init__(self): + self.location = None + + @staticmethod + def _parse_semantics(semantics_dict): + semantics = TellNameOfPerson.Semantics() + + semantics.location = resolve_entity_description(semantics_dict['location']) + + return semantics + + def _configure(self, robot, config): + self._robot = robot + + semantics = TellNameOfPerson._parse_semantics(config.semantics) + + self._sentence = "I understand you want me to tell you the name of the person at the {}. " \ + "But I can't do that yet. Sorry. Next task please.".format(semantics.location.id) + + self._config_result.succeeded = True + return + + def _start(self): + self._robot.speech.speak(self._sentence, block=True) + self._execute_result.succeeded = True + + def _cancel(self): + pass + + +if __name__ == "__main__": + rospy.init_node('say_test') + + import sys + robot_name = sys.argv[1] + if robot_name == 'amigo': + from robot_skills.amigo import Amigo as Robot + elif robot_name == 'sergio': + from robot_skills.sergio import Sergio as Robot + else: + from robot_skills.mockbot import Mockbot as Robot + + robot = Robot() + + action = CountAndTell() + + config = ConfigurationData({'action': 'count-and-tell', + 'location': {'id': 'cabinet'}, + 'object': {'type': 'beer'}}) + + action.configure(robot, config) + action.start() From 9bc40ac8a130ce5db88ef72c4da7ec35409ca553 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 11:30:49 -0400 Subject: [PATCH 04/71] feat(): expose dummy actions for asking name and counting objects --- action_server/src/action_server/actions/__init__.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py index b1c596ea..ff1e6bb4 100644 --- a/action_server/src/action_server/actions/__init__.py +++ b/action_server/src/action_server/actions/__init__.py @@ -2,6 +2,7 @@ from answer_question import AnswerQuestion from arm_goal import ArmGoal +from count_and_tell import CountAndTell from demo_presentation import DemoPresentation from find import Find from follow import Follow @@ -16,4 +17,5 @@ from reset_wm import ResetWM from say import Say from send_picture import SendPicture +from tell_name_of_person import TellNameOfPerson from turn_toward_sound import TurnTowardSound From aa0a7a532e46cc78d7cf018261f0a707b08590c9 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 11:40:46 -0400 Subject: [PATCH 05/71] refactor(answer_question): apply context structure --- .../action_server/actions/answer_question.py | 62 ++++++++++++------- 1 file changed, 40 insertions(+), 22 deletions(-) diff --git a/action_server/src/action_server/actions/answer_question.py b/action_server/src/action_server/actions/answer_question.py index 0831063c..1877255d 100644 --- a/action_server/src/action_server/actions/answer_question.py +++ b/action_server/src/action_server/actions/answer_question.py @@ -1,5 +1,6 @@ from action import Action, ConfigurationData from find import Find +from entity_description import resolve_entity_description import rospy from robocup_knowledge import load_knowledge @@ -18,6 +19,33 @@ class AnswerQuestion(Action): def __init__(self): Action.__init__(self) self._required_skills = ['speech', 'hmi'] + self._preempt_requested = False + + class Semantics: + def __init__(self): + self.target_person = None + + @staticmethod + def _parse_semantics(semantics_dict): + semantics = AnswerQuestion.Semantics() + + if 'target-person' in semantics_dict: + semantics.target_person = resolve_entity_description(semantics_dict['target-person']) + + return semantics + + class Context: + def __init__(self): + self.object_designator = None + + @staticmethod + def _parse_context(context_dict): + context = AnswerQuestion.Context() + + if 'object-designator' in context_dict: + context.object_designator = context_dict['object-designator'] + + return context def _configure(self, robot, config): self._robot = robot @@ -27,38 +55,29 @@ def _configure(self, robot, config): rospy.logerr("Failed to load speech data for 'AnswerQuestion' action") return + semantics = AnswerQuestion._parse_semantics(config.semantics) + context = AnswerQuestion._parse_context(config.context) + # If a person is specified in the task description, we need to go and find that person first - if 'target-person' in config.semantics: - self._find_action = Find() - location_id = config.semantics['target-person']['loc'] - find_semantics = {'object' : config.semantics['target-person'], - 'location' : {'id': location_id, - 'type': "furniture" if self._knowledge.is_location(location_id) - else "room"}} - find_config = ConfigurationData(find_semantics, config.knowledge) - find_config_result = self._find_action.configure(robot, find_config) - if not find_config_result.succeeded: - self._config_result = find_config_result + if semantics.target_person and not context.object_designator: + self._config_result.required_context = { + 'action': 'find', + 'object': config.semantics['target-person'] + } + if semantics.target_person.location: + self._config_result.required_context['source-location'] = config.semantics['target-person']['location'] return - else: - self._find_action = None self._config_result.succeeded = True def _start(self): - if self._find_action: - find_exec_res = self._find_action.start() - if not find_exec_res.succeeded: - self._execute_result = find_exec_res - return - self._robot.head.look_at_standing_person() self._robot.head.wait_for_motion_done() self._robot.speech.speak("What is your question?") tries = 0 - while tries < 3: + while tries < 3 and not self._preempt_requested: try: res = self._robot.hmi.query(description="", grammar=self._speech_data.grammar, @@ -90,9 +109,8 @@ def _start(self): self._execute_result.message = " I did not understand the question. " tries += 1 - def _cancel(self): - pass + self._preempt_requested = True if __name__ == "__main__": From ab09656dc8d6aecc5c7b33cbd023a569c4e16bcf Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 11:42:00 -0400 Subject: [PATCH 06/71] fix(say): use lower case keywords as upper case messes up grammar --- .../src/action_server/actions/say.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/action_server/src/action_server/actions/say.py b/action_server/src/action_server/actions/say.py index 8f6c0e02..9f4daad6 100644 --- a/action_server/src/action_server/actions/say.py +++ b/action_server/src/action_server/actions/say.py @@ -53,39 +53,39 @@ def _start(self): rospy.loginfo('Answering {}'.format(self._sentence)) # TODO: This knowledge should be somehow returned by the robot object - if self._sentence == 'TIME': + if self._sentence == 'time': hours = datetime.now().hour minutes = datetime.now().minute line = "The time is {} {}".format(hours, minutes) self._execute_result.message = " I told the time. " - elif self._sentence == "TEAM_NAME": + elif self._sentence == "team_name": line = "My team's name is Tech United Eindhoven" self._execute_result.message = " I told my team's name. " - elif self._sentence == "COUNTRY": + elif self._sentence == "country": line = "My team is from the Netherlands, also known as Holland." self._execute_result.message = " I told my team's name. " - elif self._sentence == "TEAM_AFFILIATION": + elif self._sentence == "team_affiliation": line = "My team is affiliated with the University of Technology Eindhoven" self._execute_result.message = " I told my team's affiliation. " - elif self._sentence == "ROBOT_NAME": + elif self._sentence == "robot_name": line = 'My name is {}'.format(self._robot.robot_name) self._execute_result.message = " I told my name. " - elif self._sentence == 'TODAY': + elif self._sentence == 'today': line = datetime.today().strftime('Today is %A %B %d') self._execute_result.message = " I told what day it is. " - elif self._sentence == 'TOMORROW': + elif self._sentence == 'tomorrow': line = (datetime.today() + timedelta(days=1)).strftime('Tomorrow is %A %B %d') self._execute_result.message = " I told what day it is tomorrow. " - elif self._sentence == 'DAY_OF_MONTH': + elif self._sentence == 'day_of_month': line = datetime.now().strftime('It is day %d of the month') self._execute_result.message = " I told the day of the month. " - elif self._sentence == 'DAY_OF_WEEK': + elif self._sentence == 'day_of_week': line = datetime.today().strftime('Today is a %A') self._execute_result.message = " I told the day of the week. " - elif self._sentence == 'DARK_SIDE': + elif self._sentence == 'dark_side': line = " I'll never join you! " self._execute_result.message = " I told I'll never join the dark side. " - elif self._sentence == 'JOKE': + elif self._sentence == 'joke': line = random.choice([ "What do you call a fish with no eyes? A fsh.", "You don't need a parachute to go skydiving. You need a parachute to go skydiving twice.", @@ -95,7 +95,7 @@ def _start(self): "It's color is yellow and when you push the button, it turns red? A chick in the blender" ]) self._execute_result.message = " I told a joke. " - elif self._sentence == 'SOMETHING_ABOUT_SELF': + elif self._sentence == 'something_about_self': if self._robot.robot_name == 'amigo': line = random.choice([ "I once dragged a person across the floor for meters.", From 45bf1c40a9d0adaf58391307dc7944caf10621fe Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 11:43:17 -0400 Subject: [PATCH 07/71] fix(entity_description): entity location is also entity description --- .../src/action_server/actions/entity_description.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/action_server/src/action_server/actions/entity_description.py b/action_server/src/action_server/actions/entity_description.py index fc14f5aa..be4f0a45 100644 --- a/action_server/src/action_server/actions/entity_description.py +++ b/action_server/src/action_server/actions/entity_description.py @@ -36,11 +36,9 @@ def resolve_entity_description(parameters): description.type = parameters["type"] if "category" in parameters: description.category = parameters["category"] - if "loc" in parameters: - description.location = resolve_entity_description(parameters["loc"]) + if "location" in parameters: + description.location = resolve_entity_description(parameters["location"]) if "designator" in parameters: description.designator = parameters["designator"] - if "location" in parameters: - description.location = EntityDescription(id=parameters["location"]) return description From d6783bd517b5b3d76ff7a415795ffbc6ed3e5a5b Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Sun, 17 Jun 2018 14:25:34 -0400 Subject: [PATCH 08/71] fix(find): find person not based on name --- action_server/src/action_server/actions/find.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index faa07681..407191bd 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -92,7 +92,8 @@ def _configure(self, robot, config): self._nav_areas[location] = self._knowledge.get_inspect_position(location) else: # person in room - self._find_state_machines = [states.FindPersonInRoom(robot, self._location.id, self._object.id)] + self._find_state_machines = [states.FindPersonInRoom(robot, self._location.id, self._object.id, + False)] self._areas[self._location.id] = ["in"] self._nav_areas[self._location.id] = "in" self._config_result.context['object-designator'] = self._found_entity_designator From 54935de21ad243213dc65fa09145f1b0ca740603 Mon Sep 17 00:00:00 2001 From: Sam Alexandrov Date: Sun, 17 Jun 2018 23:34:23 +0200 Subject: [PATCH 09/71] added TellMeNameOfPerson action, not tested --- .../actions/tell_name_of_person.py | 93 +++++++++++++++---- 1 file changed, 76 insertions(+), 17 deletions(-) diff --git a/action_server/src/action_server/actions/tell_name_of_person.py b/action_server/src/action_server/actions/tell_name_of_person.py index eaeb7ea4..dec05e1f 100644 --- a/action_server/src/action_server/actions/tell_name_of_person.py +++ b/action_server/src/action_server/actions/tell_name_of_person.py @@ -1,54 +1,115 @@ from action import Action, ConfigurationData +from find import Find from entity_description import resolve_entity_description import rospy +from robocup_knowledge import load_knowledge +import hmi class TellNameOfPerson(Action): - ''' The TellNameOfPerson class implements the action to ask someones name and report it to the operator. + """ The TellNameOfPerson class implements the action to ask someones name and report it to the operator. Parameters to pass to the configure() method are: - `sentence` (required): The sentence to speak. May be a keyword to tell something more intelligent. - ''' + """ + def __init__(self): Action.__init__(self) - self._required_skills = ['speech'] + self._required_skills = ['speech', 'hmi'] + self._preempt_requested = False + + common_knowledge = load_knowledge('common') + + self._grammar = "" + + for name in common_knowledge.names: + self._grammar += "\nNAME['%s'] -> %s" % (name, name) class Semantics: def __init__(self): - self.location = None + self.target_person = None @staticmethod def _parse_semantics(semantics_dict): semantics = TellNameOfPerson.Semantics() - semantics.location = resolve_entity_description(semantics_dict['location']) + if 'target-person' in semantics_dict: + semantics.target_person = resolve_entity_description(semantics_dict['target-person']) return semantics + class Context: + def __init__(self): + self.object_designator = None + + @staticmethod + def _parse_context(context_dict): + context = TellNameOfPerson.Context() + + if 'object-designator' in context_dict: + context.object_designator = context_dict['object-designator'] + + return context + def _configure(self, robot, config): self._robot = robot semantics = TellNameOfPerson._parse_semantics(config.semantics) - - self._sentence = "I understand you want me to tell you the name of the person at the {}. " \ - "But I can't do that yet. Sorry. Next task please.".format(semantics.location.id) + context = TellNameOfPerson._parse_context(config.context) + + # If a person is specified in the task description, we need to go and find that person first + if semantics.target_person and not context.object_designator: + self._config_result.required_context = { + 'action': 'find', + 'object': config.semantics['target-person'] + } + if semantics.target_person.location: + self._config_result.required_context['source-location'] = config.semantics['target-person']['location'] + return self._config_result.succeeded = True - return def _start(self): - self._robot.speech.speak(self._sentence, block=True) - self._execute_result.succeeded = True + self._robot.head.look_at_standing_person() + self._robot.head.wait_for_motion_done() + + self._robot.speech.speak("Hey sweetie what is your name?") + + tries = 0 + while tries < 3 and not self._preempt_requested: + try: + res = self._robot.hmi.query(description="", + grammar=self._grammar, + target='NAME') + except hmi.TimeoutException: + self._robot.speech.speak("My ears are not working properly, sorry!") + self._execute_result.message = "I was unable to hear anything when listening for your name" + tries += 1 + continue + + if res.semantics: + self._robot.speech.speak("Oh, so your name is {}".format(res.sentence)) + self._execute_result.message = "The person's name was {}".format(res.sentence) + self._execute_result.succeeded = True + return + else: + if tries < 2: + self._robot.speech.speak("Sorry, I did not understand you, try again.") + else: + self._robot.speech.speak("Sorry, I was unable to understand you again. I'll just name you Maple. ") + self._execute_result.message = " I did not understand the answer. " + tries += 1 def _cancel(self): - pass + self._preempt_requested = True if __name__ == "__main__": - rospy.init_node('say_test') + rospy.init_node('tell_name_of_person_test') import sys + robot_name = sys.argv[1] if robot_name == 'amigo': from robot_skills.amigo import Amigo as Robot @@ -59,11 +120,9 @@ def _cancel(self): robot = Robot() - action = CountAndTell() + action = TellNameOfPerson() - config = ConfigurationData({'action': 'count-and-tell', - 'location': {'id': 'cabinet'}, - 'object': {'type': 'beer'}}) + config = {'action': 'tell_name_of_person'} action.configure(robot, config) action.start() From 4e576a3efa4c9b1494b8a5f34312953785fba261 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Mon, 18 Jun 2018 07:58:37 -0400 Subject: [PATCH 10/71] fix(tell_name_of_person): fixes after sim testing --- .../actions/tell_name_of_person.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/action_server/src/action_server/actions/tell_name_of_person.py b/action_server/src/action_server/actions/tell_name_of_person.py index dec05e1f..89f1c66a 100644 --- a/action_server/src/action_server/actions/tell_name_of_person.py +++ b/action_server/src/action_server/actions/tell_name_of_person.py @@ -28,14 +28,14 @@ def __init__(self): class Semantics: def __init__(self): - self.target_person = None + self.location = None @staticmethod def _parse_semantics(semantics_dict): semantics = TellNameOfPerson.Semantics() - if 'target-person' in semantics_dict: - semantics.target_person = resolve_entity_description(semantics_dict['target-person']) + if 'location' in semantics_dict: + semantics.location = resolve_entity_description(semantics_dict['location']) return semantics @@ -47,8 +47,8 @@ def __init__(self): def _parse_context(context_dict): context = TellNameOfPerson.Context() - if 'object-designator' in context_dict: - context.object_designator = context_dict['object-designator'] + if 'object' in context_dict: + context.object_designator = resolve_entity_description(context_dict['object']) return context @@ -59,14 +59,15 @@ def _configure(self, robot, config): context = TellNameOfPerson._parse_context(config.context) # If a person is specified in the task description, we need to go and find that person first - if semantics.target_person and not context.object_designator: + if semantics.location and not context.object_designator: self._config_result.required_context = { 'action': 'find', - 'object': config.semantics['target-person'] + 'object': { + 'location': config.semantics['location'], + 'type': 'person' + } } - if semantics.target_person.location: - self._config_result.required_context['source-location'] = config.semantics['target-person']['location'] - return + return self._config_result.succeeded = True From 52d19e8a4030abe69c4514346418a3560a366997 Mon Sep 17 00:00:00 2001 From: Sam Alexandrov Date: Tue, 19 Jun 2018 01:23:04 +0200 Subject: [PATCH 11/71] added functionality count objects on the inspected location --- .../actions/count_object_on_location.py | 84 +++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 action_server/src/action_server/actions/count_object_on_location.py diff --git a/action_server/src/action_server/actions/count_object_on_location.py b/action_server/src/action_server/actions/count_object_on_location.py new file mode 100644 index 00000000..f73c5d8d --- /dev/null +++ b/action_server/src/action_server/actions/count_object_on_location.py @@ -0,0 +1,84 @@ +#! /usr/bin/env python +import rospy +import smach +import robot_smach_states.util.designators as ds +from robot_smach_states.util.startup import startup + +class CountObjectOnLocation(smach.StateMachine): + def __init__(self, robot, location, num_objects_designator, segmentation_area='on_top_of', object_type='', threshold=0.0): + """ Constructor + + :param robot: robot object + :param location: Where to look for objects? + :param num_objects_designator: a VariableDesignator(resolve_type=int).writeable() that will store the number of objects + :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of + :param threshold: float for classification score. Entities whose classification score is lower are ignored + (i.e. are not added to the segmented_entity_ids_designator) + """ + smach.State.__init__(self, outcomes=['done', 'failed']) + self.robot = robot + self.location = location + self.segmentation_area = segmentation_area + self.threshold = threshold + self.object = object_type + + ds.checks.is_writeable(num_objects_designator) + ds.checks.resolve_type(int) + self.num_objects = num_objects_designator + + def count_object_on_location(self): + res = self.robot.ed.update_kinect("{} {}".format(self.segmentation_area, self.location)) + segmented_object_ids = res.new_ids + res.updated_ids + + rospy.loginfo("Segmented %d objects" % len(segmented_object_ids)) + if segmented_object_ids: + object_classifications = self.robot.ed.classify(ids=segmented_object_ids) + + if object_classifications: + for idx, obj in enumerate(object_classifications): + rospy.loginfo(" - Object {} is a '{}' (ID: {})".format(idx, obj.type, obj.id)) + + if self.threshold: + over_threshold = [obj for obj in object_classifications if + obj.probability >= self.threshold] + + dropped = {obj.id: obj.probability for obj in object_classifications if + obj.probability < self.threshold} + rospy.loginfo("Dropping {l} entities due to low class. score (< {th}): {dropped}" + .format(th=self.threshold, dropped=dropped, l=len(dropped))) + + object_classifications = over_threshold + + list_objects = [obj for obj in object_classifications if obj['obj.type'] == self.object_type] + num_objects = len(list_objects) + self.num_objects_designator.write(num_objects) + return 'done' + else: + return 'failed' + + +# Standalone testing ----------------------------------------------------------------- + +class TestCountObjects(smach.StateMachine): + def __init__(self, robot): + smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) + + count = ds.VariableDesignator(0) + + with self: + smach.StateMachine.add('INITIALIZE', + states.Initialize(robot), + transitions={'initialized': 'COUNT', + 'abort': 'Aborted'}) + + smach.StateMachine.add("COUNT", + CountObjectOnLocation(robot, 'counter', object_type='coke', + num_objects_designator=count.writeable()), + transitions={'done': 'Done', + 'failed':'Aborted'}) + + +if __name__ == "__main__": + # rospy.init_node('gpsr_function_exec') + + startup(TestCountObjects, challenge_name="gps_function") From 6aeae01036ef326a091ffd1fab018e4a16c890d6 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Mon, 18 Jun 2018 20:08:46 -0400 Subject: [PATCH 12/71] some refactoring on robocup --- .../action_server/actions/answer_question.py | 8 +- .../src/action_server/actions/find.py | 206 +++++++++++------- .../src/action_server/actions/hand_over.py | 26 +-- .../src/action_server/actions/navigate_to.py | 48 ++-- .../src/action_server/actions/pick_up.py | 138 ++++++------ .../src/action_server/actions/place.py | 26 +-- .../src/action_server/task_manager.py | 6 +- 7 files changed, 262 insertions(+), 196 deletions(-) diff --git a/action_server/src/action_server/actions/answer_question.py b/action_server/src/action_server/actions/answer_question.py index 1877255d..18903491 100644 --- a/action_server/src/action_server/actions/answer_question.py +++ b/action_server/src/action_server/actions/answer_question.py @@ -36,14 +36,14 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): - self.object_designator = None + self.object = None @staticmethod def _parse_context(context_dict): context = AnswerQuestion.Context() - if 'object-designator' in context_dict: - context.object_designator = context_dict['object-designator'] + if 'object' in context_dict: + context.object = resolve_entity_description(context_dict['object']) return context @@ -59,7 +59,7 @@ def _configure(self, robot, config): context = AnswerQuestion._parse_context(config.context) # If a person is specified in the task description, we need to go and find that person first - if semantics.target_person and not context.object_designator: + if semantics.target_person and not context.object: self._config_result.required_context = { 'action': 'find', 'object': config.semantics['target-person'] diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index 407191bd..a6c70f2e 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -49,69 +49,123 @@ def _point_at_person(self, person): self._robot.rightArm.send_joint_goal('reset') + class Semantics: + def __init__(self): + self.object = None + self.source_location = None + + def __repr__(self): + return str(self.__dict__) + + @staticmethod + def _parse_semantics(semantics_dict): + semantics = Find.Semantics() + + semantics.object = resolve_entity_description(semantics_dict['object']) + + if 'source-location' in semantics_dict: + semantics.source_location = resolve_entity_description(semantics_dict['source-location']) + + return semantics + + class Context: + def __init__(self): + self.location = None + + @staticmethod + def _parse_context(context_dict): + context = Find.Context() + + if 'location' in context_dict: + context.location = resolve_entity_description(context_dict['location']) + + return context + def _configure(self, robot, config): self._robot = robot - self._object = resolve_entity_description(config.semantics['object']) - self._location = None - - if 'source-location' in config.semantics: - self._location = resolve_entity_description(config.semantics['source-location']) - elif 'location-designator' in config.context: - e = config.context['location-designator'].resolve() - if not e: - if self._object.id: - self._config_result.message = " Where should I look for the {}?".format(self._object.id) + + self._semantics = Find._parse_semantics(config.semantics) + self._context = Find._parse_context(config.context) + + # Check if we know the source location + if self._semantics.source_location: + pass + elif self._context.location: + e = self._context.location.designator.resolve() + if e: + self._semantics.source_location = self._context.location + else: + if self._semantics.object.id: + self._config_result.message = " Where should I look for the {}?".format(self._semantics.object.id) else: - self._config_result.message = " Where should I look for the {}?".format(self._object.type) + self._config_result.message = " Where should I look for the {}?".format(self._semantics.object.type) self._config_result.missing_field = 'source-location' return - self._location = EntityDescription(id=e.id) else: - self._config_result.message = " Where should I look for the {}?".format(self._object.id) + self._config_result.message = " Where should I look for the {}?".format(self._semantics.object.type) self._config_result.missing_field = 'source-location' return - # We (safely) assume that self._location is an EntityDescription object - # If we need to find a manipulable item, the location should also be manipulable - if not self._object.type == "person" and self._location.id not in self._knowledge.manipulation_locations \ - and self._location.id not in self._knowledge.location_rooms: - self._config_result.message = " I can't grasp anything from the {}".format(self._location.id) + # We can now assume we know the source location + # Set up context about the found entity + self._config_result.context['object'] = config.semantics['object'] + + # Check if we need to find a person or an object + if self._semantics.object.type == 'person': + # If no source-location is specified, check if a location is specified in the object spec + if not self._semantics.source_location: + if self._semantics.object.location: + self._semantics.source_location = self._semantics.object.location + elif self._context.location: + self._semantics.source_location = self._context.location + else: + self._config_result.message = " Where should I look for {}?".format(self._semantics.object.id) + self._config_result.missing_field = "source-location" + return + + # if we need to find the person near a furniture object, get the room where it is located + if self._semantics.source_location.id not in self._knowledge.location_rooms: + self._semantics.source_location.id = self._knowledge.get_room(self._semantics.source_location.id) + + # person in room + if not self._semantics.object.id: + self._semantics.object.id = 'someone' + self._find_state_machines = [ + states.FindPersonInRoom(robot, self._semantics.source_location.id, self._semantics.object.id, + False)] # This smach state should also return the found entity! + self._config_result.context['location'] = { + 'designator': EdEntityDesignator(self._robot, id=self._semantics.source_location.id) + } + if self._semantics.source_location.id: + self._config_result.context['location']['id'] = self._semantics.source_location.id + + self._found_entity_designator = EdEntityDesignator(self._robot, id=self._semantics.object.id) + self._config_result.context['object']['designator'] = self._found_entity_designator + self._config_result.succeeded = True return - # Set up designator to be filled with the found entity - self._found_entity_designator = VariableDesignator(resolve_type=Entity) - - # Set up designator for areas + # We need to find an object + # we need to navigate to all pieces of furniture and inspect all of their inspection areas self._areas = {} self._nav_areas = {} - if self._location.id in self._knowledge.location_rooms: - if not self._object.type == "person": - locations = self._knowledge.get_locations(self._location.id) - for location in locations: - self._areas[location] = self._knowledge.get_inspect_areas(location) - self._nav_areas[location] = self._knowledge.get_inspect_position(location) - else: - # person in room - self._find_state_machines = [states.FindPersonInRoom(robot, self._location.id, self._object.id, - False)] - self._areas[self._location.id] = ["in"] - self._nav_areas[self._location.id] = "in" - self._config_result.context['object-designator'] = self._found_entity_designator - self._config_result.context['location-designator'] = EdEntityDesignator(self._robot, id=self._location.id) - self._config_result.succeeded = True - return - elif self._object.type == "person": - self._areas[self._location.id] = ["near"] - self._nav_areas[self._location.id] = "near" + if self._semantics.source_location.id in self._knowledge.location_rooms: + locations = self._knowledge.get_locations(self._semantics.source_location.id) + for location in locations: + self._areas[location] = self._knowledge.get_inspect_areas(location) + self._nav_areas[location] = self._knowledge.get_inspect_position(location) else: - self._areas[self._location.id] = self._knowledge.get_inspect_areas(self._location.id) - self._nav_areas[self._location.id] = self._knowledge.get_inspect_position(self._location.id) + self._areas[self._semantics.source_location.id] = self._knowledge.get_inspect_areas( + self._semantics.source_location.id) + self._nav_areas[self._semantics.source_location.id] = self._knowledge.get_inspect_position( + self._semantics.source_location.id) # Set up the designator with the object description - entity_description = {'type': self._object.type} + entity_description = {'type': self._semantics.object.type, 'category': self._semantics.object.category} description_designator = VariableDesignator(entity_description) + location_designator = None self._find_state_machines = [] + self._found_entity_designator = VariableDesignator(resolve_type=Entity) for loc, areas in self._areas.iteritems(): location_designator = EdEntityDesignator(self._robot, id=loc) nav_area = self._nav_areas[loc] @@ -121,7 +175,8 @@ def _configure(self, robot, config): navigation_area_designator = VariableDesignator(nav_area) # Set up the Find state machine - rospy.loginfo("Setting up state machine with loc = {}, area = {}, nav_area = {}".format(loc, area, nav_area)) + rospy.loginfo("Setting up state machine with loc = {}, area = {}, nav_area = {}".format(loc, area, + nav_area)) self._find_state_machines.append(states.Find(robot=self._robot, source_entity_designator=location_designator, description_designator=description_designator, @@ -129,48 +184,47 @@ def _configure(self, robot, config): navigation_area_designator=navigation_area_designator, found_entity_designator=self._found_entity_designator)) - self._config_result.context['object-designator'] = self._found_entity_designator - self._config_result.context['location-designator'] = location_designator + self._config_result.context['object'] = {'designator': self._found_entity_designator, + 'type': self._semantics.object.type, + 'category': self._semantics.object.category} + self._config_result.context['location'] = {'designator': location_designator} + if self._semantics.source_location.id: + self._config_result.context['location']['id'] = self._semantics.source_location.id self._config_result.succeeded = True def _start(self): - # e = self._found_entity_designator.resolve() - # if e: - # self._execute_result.message = " I already knew where to find {}. ".format( - # self._object.id if self._object.id else "a " + self._object.type) - # self._execute_result.succeeded = True - # return - for fsm in self._find_state_machines: res = fsm.execute() if res in ['succeeded', 'found']: + self._semantics.object.type = self._found_entity_designator.resolve().type self._execute_result.message = " I found {}. ".format( - self._object.id if self._object.id else "a " + self._object.type) + self._semantics.object.id if self._semantics.object.id else "a " + self._semantics.object.type) self._execute_result.succeeded = True - if not self._object.type == "person": - self._robot.speech.speak("Hey, I found a {}!".format(self._object.type)) + # if not self._semantics.object.type == "person": + # self._robot.speech.speak("Hey, I found a {}!".format(self._semantics.object.type if self._semantics.object.type else + # self._semantics.object.category)) return - elif res == 'not_found': - if self._object.type == "person": - self._robot.speech.speak(" I don't see anyone here. ") - else: - self._robot.speech.speak("I don't see what I am looking for here.") - - self._execute_result.message = " I couldn't find {} {} the {} ".format( - self._object.id if self._object.id and not self._object.id == "None" else \ - "a " + self._object.type if self._object.type else "a " + self._object.category, - "in" if self._location.id in self._knowledge.location_rooms else "at", - self._location.id - ) - else: - self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._location.id)) - self._execute_result.message = " I was unable to inspect the {} to find {}. ".format( - self._location.id, - self._object.id if self._object.id else "a " + self._object.type if self._object.type else \ - "a " + self._object.category - ) + # elif res == 'not_found': + # if self._semantics.object.type == "person": + # self._robot.speech.speak(" I don't see anyone here. ") + # else: + # self._robot.speech.speak("I don't see what I am looking for here.") + # + # self._execute_result.message = " I couldn't find {} {} the {} ".format( + # self._semantics.object.id if self._semantics.object.id and not self._semantics.object.id == "None" else \ + # "a " + self._semantics.object.type if self._semantics.object.type else "a " + self._semantics.object.category, + # "in" if self._location.id in self._knowledge.location_rooms else "at", + # self._location.id + # ) + # else: + # self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._location.id)) + # self._execute_result.message = " I was unable to inspect the {} to find {}. ".format( + # self._location.id, + # self._semantics.object.id if self._semantics.object.id else "a " + self._semantics.object.type if self._semantics.object.type else \ + # "a " + self._semantics.object.category + # ) def _cancel(self): pass diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py index 129e1edb..d05df694 100644 --- a/action_server/src/action_server/actions/hand_over.py +++ b/action_server/src/action_server/actions/hand_over.py @@ -38,9 +38,7 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): self.arm_designator = None - self.object_designator = None # TODO: bundle object_designator and object (as a description and a ref) - self.object_type = None - self.location_designator = None + self.object = None self.location = None @staticmethod @@ -50,14 +48,8 @@ def _parse_context(context_dict): if 'arm-designator' in context_dict: context.arm_designator = context_dict['arm-designator'] - if 'object-designator' in context_dict: - context.object_designator = context_dict['object-designator'] - - if 'object-type' in context_dict: - context.object_type = context_dict['object-type'] - - if 'location-designator' in context_dict: - context.location_designator = context_dict['location-designator'] + if 'object' in context_dict: + context.object = resolve_entity_description(context_dict['object']) if 'location' in context_dict: context.location = resolve_entity_description(context_dict['location']) @@ -75,16 +67,22 @@ def _configure(self, robot, config): # We assume we already got the object if previous action passed an arm, an object and this object has the # required type, or the required type is a reference. got_object = ( - self.context.arm_designator is not None and (self.context.object_type == self.semantics.object.type or + self.context.arm_designator is not None and (self.context.object.type == self.semantics.object.type or self.semantics.object.type == 'reference')) # If precondition not met, request prior action from the task manager if not got_object: # Request pick_up action - self._config_result.required_context = {'action': 'pick-up', - 'object': config.semantics['object']} + self._config_result.required_context = {'action': 'pick-up'} + if 'object' in config.semantics and 'type' in config.semantics['object']: + if config.semantics['object']['type'] != 'reference': + self._config_result.required_context['object'] = config.semantics['object'] + elif 'object' in config.context and 'type' in config.context['object']: + self._config_result.required_context['object'] = {'type': config.context['object']['type']} if 'source-location' in config.semantics: self._config_result.required_context['source-location'] = config.semantics['source-location'] + if 'location' in config.context and 'id' in config.context['location']: + self._config_result.required_context['source-location'] = config.context['location']['id'] return # Now we can assume we picked up the item! diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py index 7db71f7e..34ee6665 100644 --- a/action_server/src/action_server/actions/navigate_to.py +++ b/action_server/src/action_server/actions/navigate_to.py @@ -39,11 +39,11 @@ def _parse_context(context_dict): # Location to navigate to if 'location' in context_dict: - context.target_location = context_dict['location'] + context.target_location = resolve_entity_description(context_dict['location']) # Object to navigate to - if 'object-designator' in context_dict: - context.object = context_dict['object-designator'] + if 'object' in context_dict: + context.object = resolve_entity_description(context_dict['object']) return context @@ -59,17 +59,30 @@ def _configure(self, robot, config): semantics = self._parse_semantics(config.semantics) context = self._parse_context(config.context) - if semantics.target_location.id == 'this_guy' and semantics.target_location.location is None: - rospy.logerr("Missing required parameter {}".format('target-location.location')) - self._config_result.missing_field = 'target-location.location' - self._config_result.message = ' Where should I look for this guy? ' - self._config_result.succeeded = False - return + # navigate to a room + know_target = False + if semantics.target_location.id in self._knowledge.rooms: + know_target = True + + # navigate to a piece of furniture + elif semantics.target_location.id in self._knowledge.location_names: + know_target = True + + # # navigate to something else that we already know in the world + # elif semantics.target_location.id and self._robot.ed.get_entity(semantics.target_location.id): + # print "3" + # know_target = True - know_target = (semantics.target_location.id and - semantics.target_location.type != 'person' or # We're talking about some piece of furniture or object - (semantics.target_location.type == 'reference' and context.object) or # navigate to "it" - (semantics.target_location.type == 'person' and context.object)) + # navigate to 'it' + elif semantics.target_location.type == 'reference' and context.object and \ + context.object.id == semantics.target_location.id: + know_target = True + + elif semantics.target_location.type == 'person' and context.object and \ + semantics.target_location.id == context.object.id or semantics.target_location.id == "operator": + know_target = True + + # {'action': 'navigate-to', 'target-location': {'type': 'person', 'id': 'john', 'location': {'id': 'counter'}}} if not know_target: # Request find action @@ -78,14 +91,14 @@ def _configure(self, robot, config): if 'type' in config.semantics['target-location'] and \ config.semantics['target-location']['type'] == 'person' and \ 'location' in config.semantics['target-location']: - self._config_result.required_context['source-location'] = {'id': config.semantics['target-location']['location']} + self._config_result.required_context['source-location'] = config.semantics['target-location']['location'] elif 'source-location' in config.semantics: self._config_result.required_context['source-location'] = config.semantics['source-location'] return # Now we can assume we know the navigation goal entity! if semantics.target_location.id and \ - semantics.target_location.type != 'person': + (semantics.target_location.type != 'person' or semantics.target_location.id == 'operator'): entity_designator = EntityByIdDesignator(self._robot, id=semantics.target_location.id) e = entity_designator.resolve() @@ -109,14 +122,15 @@ def _configure(self, robot, config): entity_designator: area}, entity_lookat_designator=entity_designator) else: - entity_designator = context.object + entity_designator = context.object.designator self._navigation_state_machine = NavigateToSymbolic(self._robot, entity_designator_area_name_map={ entity_designator: "near"}, entity_lookat_designator=entity_designator) - self._config_result.context['location-designator'] = entity_designator + self._config_result.context['location'] = config.semantics['target-location'] + self._config_result.context['location']['designator'] = entity_designator self._config_result.succeeded = True return diff --git a/action_server/src/action_server/actions/pick_up.py b/action_server/src/action_server/actions/pick_up.py index 21efeefe..b338d292 100644 --- a/action_server/src/action_server/actions/pick_up.py +++ b/action_server/src/action_server/actions/pick_up.py @@ -2,13 +2,11 @@ from util import entities_from_description from entity_description import resolve_entity_description -from find import Find import robot_smach_states from robot_smach_states.util.designators import UnoccupiedArmDesignator, EdEntityDesignator import rospy -import threading class PickUp(Action): @@ -21,105 +19,107 @@ class PickUp(Action): def __init__(self): Action.__init__(self) self._required_skills = ['arms'] - self._find_action = None + self._required_field_prompts = {'object': " What would you like me to pick up? "} - def _configure(self, robot, config): - # Check if the task included an object to grab, otherwise try to get the information from previous actions' - # context. - using_context_for_object = False - if 'object' in config.semantics: - if config.semantics['object']['type'] == 'reference': - if 'object-designator' in config.context: - using_context_for_object = True - pass - else: - self._config_result.missing_field = 'object' - self._config_result.message = " What would you like me to pick up? " - return - else: - pass + class Semantics: + def __init__(self): + self.object = None + self.source_location = None - else: - self._config_result.missing_field = 'object' - self._config_result.message = " What would you like me to pick up? " - return + @staticmethod + def _parse_semantics(semantics_dict): + semantics = PickUp.Semantics() - if not using_context_for_object: - # Check if the task included a location to grasp from, otherwise try to get the information from previous - # actions' context. - if 'source-location' in config.semantics: - pass - elif 'location-designator' in config.context: - pass - elif 'object-designator' in config.context: - using_context_for_object = True - pass - else: - self._config_result.missing_field = 'source-location' - self._config_result.message = " Where would you like me to pick that up? " - return - - if not using_context_for_object: - # Only filter to entities that can be picked up, e.g not furniture etc - manipulable_object_types = [o['name'] for o in self._knowledge.objects] - if not config.semantics['object']['type'] in manipulable_object_types: - self._config_result.message = " A {} is not something I can pick up. ".\ - format(config.semantics['object']['type']) - return - - self._find_action = Find() - find_config = ConfigurationData(config.semantics, config.context) - find_config_result = self._find_action.configure(robot, find_config) - if not find_config_result.succeeded: - self._config_result = find_config_result - return - config.context['object-designator'] = find_config_result.context['object-designator'] + semantics.object = resolve_entity_description(semantics_dict['object']) - # Add the found object to the context that is passed to the next task - self._config_result.context['object-designator'] = config.context['object-designator'] + if 'source-location' in semantics_dict: + semantics.source_location = resolve_entity_description(semantics_dict['source-location']) + + return semantics + + class Context: + def __init__(self): + self.arm_designator = None + self.object = None + self.location = None + + @staticmethod + def _parse_context(context_dict): + context = PickUp.Context() + + if 'arm-designator' in context_dict: + context.arm_designator = context_dict['arm-designator'] + if 'object' in context_dict: + context.object = resolve_entity_description(context_dict['object']) + + if 'location' in context_dict: + context.location = resolve_entity_description(context_dict['location']) + + return context + + def _configure(self, robot, config): self._robot = robot + semantics = PickUp._parse_semantics(config.semantics) + context = PickUp._parse_context(config.context) + + # Check if a previous action had us find the object already... + object_found = False + if context.object: # we got some object from a previous action + if semantics.object.type: # we got some sort of type, could still be a reference... + if semantics.object.type != 'reference': # an object type was specified in the task + if semantics.object.type == context.object.type: # then they need to be the same + object_found = True + else: # the object was a reference, then we don't care what the type of context object is + object_found = True + elif semantics.object.category: # no type, but a category was specified + # Then the context object must be one of that category + if self._knowledge.get_object_category(context.object.type) == semantics.object.category: + object_found = True + + if not object_found: + self._config_result.required_context = { + 'action': 'find', + 'object': config.semantics['object'] + } + if semantics.source_location: + self._config_result.required_context['source-location'] = config.semantics['source-location'] + return + + # Add the found object to the context that is passed to the next task + self._config_result.context['object'] = config.context['object'] + side = config.semantics['side'] if 'side' in config.semantics else 'right' arm_des = UnoccupiedArmDesignator(self._robot.arms, self._robot.arms[side]).lockable() arm_des.lock() self._fsm = robot_smach_states.grab.Grab(self._robot, - item=config.context['object-designator'], + item=context.object.designator, arm=arm_des) self._config_result.context['arm-designator'] = arm_des - self._config_result.context['object-type'] = config.semantics['object']['type'] self._config_result.succeeded = True def _start(self): - if self._find_action: - find_action_result = self._find_action.start() - - self._execute_result.message += find_action_result.message - if not find_action_result.succeeded: - return - fsm_result = self._fsm.execute() if fsm_result == "done": self._execute_result.succeeded = True - if self._find_action: - self._execute_result.message += " And I picked it up. " - elif not self._config_result.context['object-designator'].resolve(): + if not self._config_result.context['object']['designator'].resolve(): self._execute_result.message += " I could not pick anything up. ".\ format(self._config_result.context) else: self._execute_result.message += " I picked up the {}. ".\ - format(self._config_result.context['object-designator'].resolve().type) + format(self._config_result.context['object']['designator'].resolve().type) else: - if not self._config_result.context['object-designator'].resolve(): + if not self._config_result.context['object']['designator'].resolve(): self._execute_result.message += " I could not pick anything up. ". \ format(self._config_result.context) else: self._execute_result.message += " I could not pick up the {}. ".\ - format(self._config_result.context['object-designator'].resolve().type) + format(self._config_result.context['object']['designator'].resolve().type) def _cancel(self): if self._fsm.is_running: diff --git a/action_server/src/action_server/actions/place.py b/action_server/src/action_server/actions/place.py index 0f1b285a..e19647ec 100644 --- a/action_server/src/action_server/actions/place.py +++ b/action_server/src/action_server/actions/place.py @@ -51,9 +51,7 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): self.arm_designator = None - self.object_designator = None - self.object_type = None - self.location_designator = None + self.object = None self.location = None @staticmethod @@ -63,14 +61,8 @@ def _parse_context(context_dict): if 'arm-designator' in context_dict: context.arm_designator = context_dict['arm-designator'] - if 'object-designator' in context_dict: - context.object_designator = context_dict['object-designator'] - - if 'object-type' in context_dict: - context.object_type = context_dict['object-type'] - - if 'location-designator' in context_dict: - context.location_designator = context_dict['location-designator'] + if 'object' in context_dict: + context.object = resolve_entity_description(context_dict['object']) if 'location' in context_dict: context.location = resolve_entity_description(context_dict['location']) @@ -88,7 +80,7 @@ def _configure(self, robot, config): # We assume we already got the object if previous action passed an arm, an object and this object has the # required type, or the required type is a reference. got_object_in_task = ( - self.context.arm_designator is not None and (self.context.object_type == self.semantics.object.type or + self.context.arm_designator is not None and (self.context.object.type == self.semantics.object.type or self.semantics.object.type == 'reference')) object_in_gripper = False @@ -107,10 +99,16 @@ def _configure(self, robot, config): # If precondition not met, request prior action from the task manager if not got_object: # Request pick_up action - self._config_result.required_context = {'action': 'pick-up', - 'object': config.semantics['object']} + self._config_result.required_context = {'action': 'pick-up'} + if 'object' in config.semantics and 'type' in config.semantics['object']: + if config.semantics['object']['type'] != 'reference': + self._config_result.required_context['object'] = config.semantics['object'] + elif 'object' in config.context and 'type' in config.context['object']: + self._config_result.required_context['object'] = {'type': config.context['object']['type']} if 'source-location' in config.semantics: self._config_result.required_context['source-location'] = config.semantics['source-location'] + if 'location' in config.context and 'id' in config.context['location']: + self._config_result.required_context['source-location'] = config.context['location']['id'] return # Now we can assume we picked up the item! diff --git a/action_server/src/action_server/task_manager.py b/action_server/src/action_server/task_manager.py index a012f3b1..1cd235c2 100644 --- a/action_server/src/action_server/task_manager.py +++ b/action_server/src/action_server/task_manager.py @@ -41,13 +41,14 @@ def recursive_configure(self, action, configuration_data): # If context is required, we will need to configure an action prior to the current one if configuration_result.required_context: + rospy.logwarn("More context required: {}".format(configuration_result.required_context)) + # We derive the required action from the required context # TODO: Using postconditions of actions, we could infer what kind of prior action is necessary prior_action = self.get_action_from_context(configuration_result.required_context) # Configure the prior action using the same recursive strategy - prior_config_data = ConfigurationData(configuration_result.required_context, - configuration_data.context) + prior_config_data = ConfigurationData(configuration_result.required_context) prior_action_list, prior_action_config_result = self.recursive_configure(prior_action, prior_config_data) @@ -66,6 +67,7 @@ def recursive_configure(self, action, configuration_data): else: if configuration_result.succeeded: action_list.append(action) + rospy.loginfo("Configuration succeeded!\n") preconditions_met = True # Return the list of configured actions and the current configuration result From d3f1558a9ab2a4227b9803ba7eb3234cfb45a059 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Jun 2018 20:10:40 -0400 Subject: [PATCH 13/71] Add state machine to count objects and write them to a VariableDesignator --- .../actions/count_object_on_location.py | 34 ++++++++++--------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/action_server/src/action_server/actions/count_object_on_location.py b/action_server/src/action_server/actions/count_object_on_location.py index f73c5d8d..6e909db2 100644 --- a/action_server/src/action_server/actions/count_object_on_location.py +++ b/action_server/src/action_server/actions/count_object_on_location.py @@ -1,10 +1,11 @@ #! /usr/bin/env python import rospy import smach +import robot_smach_states as states import robot_smach_states.util.designators as ds from robot_smach_states.util.startup import startup -class CountObjectOnLocation(smach.StateMachine): +class CountObjectOnLocation(smach.State): def __init__(self, robot, location, num_objects_designator, segmentation_area='on_top_of', object_type='', threshold=0.0): """ Constructor @@ -20,13 +21,13 @@ def __init__(self, robot, location, num_objects_designator, segmentation_area='o self.location = location self.segmentation_area = segmentation_area self.threshold = threshold - self.object = object_type + self.object_type = object_type ds.checks.is_writeable(num_objects_designator) - ds.checks.resolve_type(int) - self.num_objects = num_objects_designator + ds.checks.check_resolve_type(num_objects_designator, int) + self.num_objects_designator = num_objects_designator - def count_object_on_location(self): + def execute(self, userdata=None): res = self.robot.ed.update_kinect("{} {}".format(self.segmentation_area, self.location)) segmented_object_ids = res.new_ids + res.updated_ids @@ -36,21 +37,22 @@ def count_object_on_location(self): if object_classifications: for idx, obj in enumerate(object_classifications): - rospy.loginfo(" - Object {} is a '{}' (ID: {})".format(idx, obj.type, obj.id)) + rospy.loginfo(" - Object {i} is a '{t}' (prob: {p}, ID: {id})".format(i=idx, t=obj.type, id=obj.id, p=obj.probability)) + over_threshold = object_classifications if self.threshold: - over_threshold = [obj for obj in object_classifications if - obj.probability >= self.threshold] + over_threshold = [obj for obj in object_classifications if obj.probability >= self.threshold] - dropped = {obj.id: obj.probability for obj in object_classifications if - obj.probability < self.threshold} + dropped = {obj.id: obj.probability for obj in object_classifications if obj.probability < self.threshold} rospy.loginfo("Dropping {l} entities due to low class. score (< {th}): {dropped}" .format(th=self.threshold, dropped=dropped, l=len(dropped))) + import ipdb; ipdb.set_trace() object_classifications = over_threshold - list_objects = [obj for obj in object_classifications if obj['obj.type'] == self.object_type] + list_objects = [obj for obj in object_classifications if obj.type == self.object_type] num_objects = len(list_objects) + rospy.loginfo("Counted {} objects matching the query".format(num_objects)) self.num_objects_designator.write(num_objects) return 'done' else: @@ -63,7 +65,7 @@ class TestCountObjects(smach.StateMachine): def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) - count = ds.VariableDesignator(0) + count = ds.VariableDesignator(-1) with self: smach.StateMachine.add('INITIALIZE', @@ -72,13 +74,13 @@ def __init__(self, robot): 'abort': 'Aborted'}) smach.StateMachine.add("COUNT", - CountObjectOnLocation(robot, 'counter', object_type='coke', - num_objects_designator=count.writeable()), + CountObjectOnLocation(robot, 'counter', object_type='apple', + num_objects_designator=count.writeable), transitions={'done': 'Done', 'failed':'Aborted'}) if __name__ == "__main__": - # rospy.init_node('gpsr_function_exec') + rospy.init_node('gpsr_function_exec') - startup(TestCountObjects, challenge_name="gps_function") + startup(TestCountObjects, challenge_name="gpsr_function") From 7d4c6b0f2c154d5b5abed03840567fd7bc387023 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Jun 2018 20:11:16 -0400 Subject: [PATCH 14/71] Remove debug statement --- .../src/action_server/actions/count_object_on_location.py | 1 - 1 file changed, 1 deletion(-) diff --git a/action_server/src/action_server/actions/count_object_on_location.py b/action_server/src/action_server/actions/count_object_on_location.py index 6e909db2..183bebb9 100644 --- a/action_server/src/action_server/actions/count_object_on_location.py +++ b/action_server/src/action_server/actions/count_object_on_location.py @@ -47,7 +47,6 @@ def execute(self, userdata=None): rospy.loginfo("Dropping {l} entities due to low class. score (< {th}): {dropped}" .format(th=self.threshold, dropped=dropped, l=len(dropped))) - import ipdb; ipdb.set_trace() object_classifications = over_threshold list_objects = [obj for obj in object_classifications if obj.type == self.object_type] From 1058c35f5751b3af7c759a597bb6da8fa2e47887 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Jun 2018 20:59:11 -0400 Subject: [PATCH 15/71] Move CountObjectOnLocation to robot_smach_states and wrap that state machine in an Action See commit 4a8c34fca1d07a59ac75976390fc76dcd8faba6e of https://www.github.com/tue-robotics/tue_robocup --- .../action_server/actions/count_and_tell.py | 22 +++-- .../actions/count_object_on_location.py | 85 ------------------- 2 files changed, 16 insertions(+), 91 deletions(-) delete mode 100644 action_server/src/action_server/actions/count_object_on_location.py diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index 2deffe50..cac13e0a 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -2,6 +2,9 @@ from entity_description import resolve_entity_description import rospy +import robot_smach_states as states +import robot_smach_states.util.designators as ds + class CountAndTell(Action): @@ -33,17 +36,24 @@ def _configure(self, robot, config): semantics = CountAndTell._parse_semantics(config.semantics) - self._sentence = "I understand you want me to count the number of {} on the {} and tell you. " \ - "But I can't do that yet. Sorry. Next task please.".format(semantics.object.type, - semantics.location.id) + self._count_designator = ds.VariableDesignator(-1) + self._count_state_machine = states.CountObjectsOnLocation(robot, + location=semantics.location.id, + num_objects_designator=self._count_designator.writeable, + object_type=semantics.object.type) + # Here we set up a message that is formatted further later, in self._start + self._execute_result.message = "I counted {{c}} {t}s".format(t=semantics.object.type) self._config_result.succeeded = True return def _start(self): - self._robot.speech.speak(self._sentence, block=True) + self._count_state_machine.execute() self._execute_result.succeeded = True + # This message is instantiated in _configure but leaves some stuff to be formatted + self._execute_result.message = self._execute_result.message.format(c=self._count_designator.resolve()) + def _cancel(self): pass @@ -65,8 +75,8 @@ def _cancel(self): action = CountAndTell() config = ConfigurationData({'action': 'count-and-tell', - 'location': {'id': 'cabinet'}, - 'object': {'type': 'beer'}}) + 'location': {'id': 'counter'}, + 'object': {'type': 'apple'}}) action.configure(robot, config) action.start() diff --git a/action_server/src/action_server/actions/count_object_on_location.py b/action_server/src/action_server/actions/count_object_on_location.py deleted file mode 100644 index 183bebb9..00000000 --- a/action_server/src/action_server/actions/count_object_on_location.py +++ /dev/null @@ -1,85 +0,0 @@ -#! /usr/bin/env python -import rospy -import smach -import robot_smach_states as states -import robot_smach_states.util.designators as ds -from robot_smach_states.util.startup import startup - -class CountObjectOnLocation(smach.State): - def __init__(self, robot, location, num_objects_designator, segmentation_area='on_top_of', object_type='', threshold=0.0): - """ Constructor - - :param robot: robot object - :param location: Where to look for objects? - :param num_objects_designator: a VariableDesignator(resolve_type=int).writeable() that will store the number of objects - :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of - :param threshold: float for classification score. Entities whose classification score is lower are ignored - (i.e. are not added to the segmented_entity_ids_designator) - """ - smach.State.__init__(self, outcomes=['done', 'failed']) - self.robot = robot - self.location = location - self.segmentation_area = segmentation_area - self.threshold = threshold - self.object_type = object_type - - ds.checks.is_writeable(num_objects_designator) - ds.checks.check_resolve_type(num_objects_designator, int) - self.num_objects_designator = num_objects_designator - - def execute(self, userdata=None): - res = self.robot.ed.update_kinect("{} {}".format(self.segmentation_area, self.location)) - segmented_object_ids = res.new_ids + res.updated_ids - - rospy.loginfo("Segmented %d objects" % len(segmented_object_ids)) - if segmented_object_ids: - object_classifications = self.robot.ed.classify(ids=segmented_object_ids) - - if object_classifications: - for idx, obj in enumerate(object_classifications): - rospy.loginfo(" - Object {i} is a '{t}' (prob: {p}, ID: {id})".format(i=idx, t=obj.type, id=obj.id, p=obj.probability)) - - over_threshold = object_classifications - if self.threshold: - over_threshold = [obj for obj in object_classifications if obj.probability >= self.threshold] - - dropped = {obj.id: obj.probability for obj in object_classifications if obj.probability < self.threshold} - rospy.loginfo("Dropping {l} entities due to low class. score (< {th}): {dropped}" - .format(th=self.threshold, dropped=dropped, l=len(dropped))) - - object_classifications = over_threshold - - list_objects = [obj for obj in object_classifications if obj.type == self.object_type] - num_objects = len(list_objects) - rospy.loginfo("Counted {} objects matching the query".format(num_objects)) - self.num_objects_designator.write(num_objects) - return 'done' - else: - return 'failed' - - -# Standalone testing ----------------------------------------------------------------- - -class TestCountObjects(smach.StateMachine): - def __init__(self, robot): - smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) - - count = ds.VariableDesignator(-1) - - with self: - smach.StateMachine.add('INITIALIZE', - states.Initialize(robot), - transitions={'initialized': 'COUNT', - 'abort': 'Aborted'}) - - smach.StateMachine.add("COUNT", - CountObjectOnLocation(robot, 'counter', object_type='apple', - num_objects_designator=count.writeable), - transitions={'done': 'Done', - 'failed':'Aborted'}) - - -if __name__ == "__main__": - rospy.init_node('gpsr_function_exec') - - startup(TestCountObjects, challenge_name="gpsr_function") From 9b86b6d44ca5b264b89b9dafea86fa967b32ca03 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Jun 2018 21:12:29 -0400 Subject: [PATCH 16/71] Include the result when debugging --- action_server/src/action_server/actions/count_and_tell.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index cac13e0a..3c000b1c 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -80,3 +80,5 @@ def _cancel(self): action.configure(robot, config) action.start() + + rospy.loginfo(action._execute_result) From 9faac8fad1a9c3e6e1634b9d033af7c7cf12ac86 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Mon, 18 Jun 2018 22:58:32 -0400 Subject: [PATCH 17/71] fix(find): move to found person --- .../src/action_server/actions/find.py | 80 ++++++++++++------- 1 file changed, 51 insertions(+), 29 deletions(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index a6c70f2e..6cc315e7 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -39,7 +39,6 @@ def _point_at_person(self, person): self._robot.head.cancel_goal() self._robot.base.force_drive(0, 0, math.copysign(1, th) * vth, abs(th / vth)) - self._robot.speech.speak("Found you!") self._robot.speech.speak("I will point at you now.") self._robot.head.look_at_ground_in_front_of_robot(distance=100) @@ -130,9 +129,10 @@ def _configure(self, robot, config): # person in room if not self._semantics.object.id: self._semantics.object.id = 'someone' + self._found_entity_designator = VariableDesignator(resolve_type=Entity) self._find_state_machines = [ states.FindPersonInRoom(robot, self._semantics.source_location.id, self._semantics.object.id, - False)] # This smach state should also return the found entity! + False, self._found_entity_designator.writeable)] # This smach state should also return the found entity! self._config_result.context['location'] = { 'designator': EdEntityDesignator(self._robot, id=self._semantics.source_location.id) } @@ -142,6 +142,14 @@ def _configure(self, robot, config): self._found_entity_designator = EdEntityDesignator(self._robot, id=self._semantics.object.id) self._config_result.context['object']['designator'] = self._found_entity_designator self._config_result.succeeded = True + + # TODO: Robocup hack to make sure the robot moves to the found person + self._navigation_state_machine = states.NavigateToWaypoint( + self._robot, + waypoint_designator=self._found_entity_designator, + radius=0.7, + look_at_designator=self._found_entity_designator + ) return # We need to find an object @@ -196,35 +204,49 @@ def _start(self): for fsm in self._find_state_machines: res = fsm.execute() + location = None + if self._semantics.source_location.id: + location = self._semantics.source_location.id + if res in ['succeeded', 'found']: - self._semantics.object.type = self._found_entity_designator.resolve().type - self._execute_result.message = " I found {}. ".format( - self._semantics.object.id if self._semantics.object.id else "a " + self._semantics.object.type) - self._execute_result.succeeded = True - - # if not self._semantics.object.type == "person": - # self._robot.speech.speak("Hey, I found a {}!".format(self._semantics.object.type if self._semantics.object.type else - # self._semantics.object.category)) + if self._semantics.object.type == 'person': + if self._semantics.object.id: + if location: + self._execute_result.message = " I found {} at the {}. ".format(self._semantics.object.id, + location) + else: + self._execute_result.message = " I found {}. ".format(self._semantics.object.id) + else: + if location: + self._execute_result.message = " I found a person at the {}. ".format(location) + else: + self._execute_result.message = " I found a person. " + self._point_at_person(self._found_entity_designator.resolve()) + self._navigation_state_machine.execute() + else: + self._robot.speech.speak("Hey, I found a {}!".format(self._semantics.object.type if + self._semantics.object.type else + self._semantics.object.category)) return - # elif res == 'not_found': - # if self._semantics.object.type == "person": - # self._robot.speech.speak(" I don't see anyone here. ") - # else: - # self._robot.speech.speak("I don't see what I am looking for here.") - # - # self._execute_result.message = " I couldn't find {} {} the {} ".format( - # self._semantics.object.id if self._semantics.object.id and not self._semantics.object.id == "None" else \ - # "a " + self._semantics.object.type if self._semantics.object.type else "a " + self._semantics.object.category, - # "in" if self._location.id in self._knowledge.location_rooms else "at", - # self._location.id - # ) - # else: - # self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._location.id)) - # self._execute_result.message = " I was unable to inspect the {} to find {}. ".format( - # self._location.id, - # self._semantics.object.id if self._semantics.object.id else "a " + self._semantics.object.type if self._semantics.object.type else \ - # "a " + self._semantics.object.category - # ) + elif res == 'not_found': + if self._semantics.object.type == "person": + self._robot.speech.speak(" I don't see anyone here. ") + else: + self._robot.speech.speak("I don't see what I am looking for here.") + + self._execute_result.message = " I couldn't find {} {} the {} ".format( + self._semantics.object.id if self._semantics.object.id and not self._semantics.object.id == "None" else \ + "a " + self._semantics.object.type if self._semantics.object.type else "a " + self._semantics.object.category, + "in" if self._location.id in self._knowledge.location_rooms else "at", + self._location.id + ) + else: + self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._location.id)) + self._execute_result.message = " I was unable to inspect the {} to find {}. ".format( + self._location.id, + self._semantics.object.id if self._semantics.object.id else "a " + self._semantics.object.type if self._semantics.object.type else \ + "a " + self._semantics.object.category + ) def _cancel(self): pass From d0eec6cc93832d2cd30edc2fee14ed11bae5f77f Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Mon, 18 Jun 2018 23:28:04 -0400 Subject: [PATCH 18/71] fix(find): add execute result succeeded --- action_server/src/action_server/actions/find.py | 1 + 1 file changed, 1 insertion(+) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index 6cc315e7..7d73d5f8 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -227,6 +227,7 @@ def _start(self): self._robot.speech.speak("Hey, I found a {}!".format(self._semantics.object.type if self._semantics.object.type else self._semantics.object.category)) + self._execute_result.succeeded = True return elif res == 'not_found': if self._semantics.object.type == "person": From efa275610c94093f41883637decc34f23a43d2db Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Mon, 18 Jun 2018 23:28:58 -0400 Subject: [PATCH 19/71] fix(tell_name_of_person): some context fixes --- .../action_server/actions/tell_name_of_person.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/action_server/src/action_server/actions/tell_name_of_person.py b/action_server/src/action_server/actions/tell_name_of_person.py index 89f1c66a..6c442230 100644 --- a/action_server/src/action_server/actions/tell_name_of_person.py +++ b/action_server/src/action_server/actions/tell_name_of_person.py @@ -41,14 +41,14 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): - self.object_designator = None + self.object = None @staticmethod def _parse_context(context_dict): context = TellNameOfPerson.Context() if 'object' in context_dict: - context.object_designator = resolve_entity_description(context_dict['object']) + context.object = resolve_entity_description(context_dict['object']) return context @@ -59,13 +59,11 @@ def _configure(self, robot, config): context = TellNameOfPerson._parse_context(config.context) # If a person is specified in the task description, we need to go and find that person first - if semantics.location and not context.object_designator: + if semantics.location and not context.object: self._config_result.required_context = { 'action': 'find', - 'object': { - 'location': config.semantics['location'], - 'type': 'person' - } + 'object': {'type': 'person'}, + 'source-location': config.semantics['location'], } return @@ -90,7 +88,7 @@ def _start(self): continue if res.semantics: - self._robot.speech.speak("Oh, so your name is {}".format(res.sentence)) + self._robot.speech.speak("Hi {name}! Bye {name}".format(name=res.sentence)) self._execute_result.message = "The person's name was {}".format(res.sentence) self._execute_result.succeeded = True return From 15530cb81150dfc3b34efe503ba5ddb982612b95 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 18 Jun 2018 23:31:14 -0400 Subject: [PATCH 20/71] change state_machine for count and tell --- .../src/action_server/actions/count_and_tell.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index 3c000b1c..94160d11 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -37,10 +37,12 @@ def _configure(self, robot, config): semantics = CountAndTell._parse_semantics(config.semantics) self._count_designator = ds.VariableDesignator(-1) - self._count_state_machine = states.CountObjectsOnLocation(robot, - location=semantics.location.id, - num_objects_designator=self._count_designator.writeable, - object_type=semantics.object.type) + self._where_to_count_designator = ds.EntityByIdDesignator(robot, id=semantics.location.id) + self._what_to_count_designator = ds.Designator(semantics.object.type) + self._count_state_machine = states.InspectAndCount(robot, + self._where_to_count_designator, + self._what_to_count_designator, + self._count_designator) # Here we set up a message that is formatted further later, in self._start self._execute_result.message = "I counted {{c}} {t}s".format(t=semantics.object.type) From 19e85058a6bebf8e737c9074ad304b28e0a78919 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 08:09:31 -0400 Subject: [PATCH 21/71] fix(count_and_tell): clearer report of counting --- action_server/src/action_server/actions/count_and_tell.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index 94160d11..05cd509f 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -45,7 +45,8 @@ def _configure(self, robot, config): self._count_designator) # Here we set up a message that is formatted further later, in self._start - self._execute_result.message = "I counted {{c}} {t}s".format(t=semantics.object.type) + self._execute_result.message = "I counted {{c}} {t}s on the {l}".format(t=semantics.object.type, + l=semantics.location.id) self._config_result.succeeded = True return @@ -55,6 +56,7 @@ def _start(self): # This message is instantiated in _configure but leaves some stuff to be formatted self._execute_result.message = self._execute_result.message.format(c=self._count_designator.resolve()) + self._robot.speech.speak(self._execute_result.message) def _cancel(self): pass From 24df4ca7d14da9f57c818f8d0f3da13c04814f87 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 08:11:41 -0400 Subject: [PATCH 22/71] fix(find): only find on manipulation locations --- action_server/src/action_server/actions/find.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index 7d73d5f8..c160fdb5 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -157,7 +157,7 @@ def _configure(self, robot, config): self._areas = {} self._nav_areas = {} if self._semantics.source_location.id in self._knowledge.location_rooms: - locations = self._knowledge.get_locations(self._semantics.source_location.id) + locations = self._knowledge.get_locations(self._semantics.source_location.id, True) for location in locations: self._areas[location] = self._knowledge.get_inspect_areas(location) self._nav_areas[location] = self._knowledge.get_inspect_position(location) From e84923e3b5418d4b5dfd4eb28242d3d2fd317b7d Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 08:12:09 -0400 Subject: [PATCH 23/71] fix(find): clearer reporting --- action_server/src/action_server/actions/find.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index c160fdb5..b5392a76 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -223,10 +223,14 @@ def _start(self): self._execute_result.message = " I found a person. " self._point_at_person(self._found_entity_designator.resolve()) self._navigation_state_machine.execute() + self._robot.speech.speak("Hi there!") else: self._robot.speech.speak("Hey, I found a {}!".format(self._semantics.object.type if self._semantics.object.type else self._semantics.object.category)) + self._execute_result.message = " I found a {} ".format(self._semantics.object.type if + self._semantics.object.type else + self._semantics.object.category) self._execute_result.succeeded = True return elif res == 'not_found': From 95868d07ce8c24bf872c74416feafe8b2d536e6f Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 08:12:38 -0400 Subject: [PATCH 24/71] fix(find): remove references to unassigned self._location --- action_server/src/action_server/actions/find.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index b5392a76..af48f495 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -242,13 +242,13 @@ def _start(self): self._execute_result.message = " I couldn't find {} {} the {} ".format( self._semantics.object.id if self._semantics.object.id and not self._semantics.object.id == "None" else \ "a " + self._semantics.object.type if self._semantics.object.type else "a " + self._semantics.object.category, - "in" if self._location.id in self._knowledge.location_rooms else "at", - self._location.id + "in" if self._semantics.source_location.id in self._knowledge.location_rooms else "at", + self._semantics.source_location.id ) else: - self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._location.id)) + self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._semantics.source_location.id)) self._execute_result.message = " I was unable to inspect the {} to find {}. ".format( - self._location.id, + self._semantics.source_location.id, self._semantics.object.id if self._semantics.object.id else "a " + self._semantics.object.type if self._semantics.object.type else \ "a " + self._semantics.object.category ) From 2b0105c736e0cf17a5b911e8902595695db146f3 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 09:19:20 -0400 Subject: [PATCH 25/71] fix(count_and_tell): report if failed --- action_server/src/action_server/actions/count_and_tell.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index 05cd509f..067469ab 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -51,7 +51,12 @@ def _configure(self, robot, config): return def _start(self): - self._count_state_machine.execute() + res = self._count_state_machine.execute() + if res == 'Aborted': + self._execute_result.succeeded = False + self._execute_result.message = "I failed to count the objects." + return + self._execute_result.succeeded = True # This message is instantiated in _configure but leaves some stuff to be formatted From 22e1eb1badfd777014724f4fa63557855d279820 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Tue, 19 Jun 2018 09:49:23 -0400 Subject: [PATCH 26/71] no logic here and fixed is_running bug --- action_server/src/action_server/actions/send_picture.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py index 291d77fc..c356e4f8 100644 --- a/action_server/src/action_server/actions/send_picture.py +++ b/action_server/src/action_server/actions/send_picture.py @@ -58,8 +58,6 @@ def _configure(self, robot, config): return def _start(self): - self._robot.speech.speak("I will take a picture and send it to my operator now. ") - result = self.detect_face_state_machine.execute(self._robot) if result == 'failed': @@ -70,8 +68,7 @@ def _start(self): self._execute_result.succeeded = True def _cancel(self): - if self.detect_face_state_machine.is_running: - self.detect_face_state_machine.request_preempt() + return if __name__ == "__main__": From 7bf04d46f37ae1960b41dcc336e722b2d346a525 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 09:58:25 -0400 Subject: [PATCH 27/71] fix(send_picture): use new context structure --- action_server/src/action_server/actions/send_picture.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py index c356e4f8..e51cf41d 100644 --- a/action_server/src/action_server/actions/send_picture.py +++ b/action_server/src/action_server/actions/send_picture.py @@ -27,14 +27,14 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): - self.location_designator = None + self.location = None @staticmethod def _parse_context(context_dict): context = SendPicture.Context() - if 'location-designator' in context_dict: - context.location_designator = context_dict['location-designator'] + if 'location' in context_dict: + context.location = resolve_entity_description(context_dict['location']) return context @@ -44,7 +44,7 @@ def _configure(self, robot, config): self.semantics = self._parse_semantics(config.semantics) self.context = self._parse_context(config.context) - if self.context.location_designator is None: + if self.context.location is None: # Request navigation action self._config_result.required_context = {'action': 'navigate-to'} if self.semantics.target_location is not None: From 53ecffe2ff1e948d19b0d1fa86a3d5ded1ad0e57 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 10:34:26 -0400 Subject: [PATCH 28/71] fix(find): increase radius of found person --- action_server/src/action_server/actions/find.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index af48f495..e48e6630 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -147,7 +147,7 @@ def _configure(self, robot, config): self._navigation_state_machine = states.NavigateToWaypoint( self._robot, waypoint_designator=self._found_entity_designator, - radius=0.7, + radius=1.0, look_at_designator=self._found_entity_designator ) return From a517a5cb4d06f0d2391b53959f1962980dfce651 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 11:03:19 -0400 Subject: [PATCH 29/71] fix(guide): check if found a person --- .../src/action_server/actions/guide.py | 33 +++++++++++-------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 2f0e8116..8631d5da 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -21,7 +21,7 @@ def __init__(self): class Semantics: def __init__(self): - self.follower = None + self.object = None self.source_location = None self.target_location = None @@ -29,8 +29,7 @@ def __init__(self): def _parse_semantics(semantics_dict): semantics = Guide.Semantics() - if 'object' in semantics_dict: - semantics.follower = resolve_entity_description(semantics_dict['object']) + semantics.object = resolve_entity_description(semantics_dict['object']) if 'source-location' in semantics_dict: semantics.source_location = resolve_entity_description(semantics_dict['source-location']) @@ -42,38 +41,46 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): - self.person_designator = None + self.person = None @staticmethod def _parse_context(context_dict): context = Guide.Context() - if 'person-designator' in context_dict: - context.person_designator = context_dict['person-designator'] + if 'person' in context_dict: + context.person = resolve_entity_description(context_dict['person']) return context def _configure(self, robot, config): + self._robot = robot + # We start by parsing semantics and context self._semantics = Guide._parse_semantics(config.semantics) self._context = Guide._parse_context(config.context) + person_known = False + if self._robot.ed.get_entity(id=self._semantics.object.id): + person_known = True + elif self._context.person: + person_known = True + # Before we start guiding, we need to know we're at the person we should guide # we check this by checking if we found a person earlier - if not self._context.person_designator: + if not person_known: self._config_result.required_context = { 'action': 'find', - 'object': config.semantics['object'], - 'source-location': config.semantics['source-location'] + 'object': config.semantics['object'] } - print "I'll need to find someone first!" + if self._semantics.source_location: + self._config_result.required_context['source-location'] = config.semantics['source-location'] return - target_location_designator = ds.EntityByIdDesignator(robot, id=self._semantics.target_location.id) + target_location_designator = ds.EntityByIdDesignator(self._robot, id=self._semantics.target_location.id) - self._guide_state_machine = states.Guide(robot=robot, + self._guide_state_machine = states.Guide(robot=self._robot, target_location=target_location_designator, - follower=self._context.person_designator) + follower=self._context.person.designator) self._config_result.succeeded = True From bef663359eaace6b30220af8c1246cd8c7658415 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 12:27:56 -0400 Subject: [PATCH 30/71] fix(navigate_to): fix waypoint navigation --- action_server/src/action_server/actions/navigate_to.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py index 34ee6665..22eeb0e6 100644 --- a/action_server/src/action_server/actions/navigate_to.py +++ b/action_server/src/action_server/actions/navigate_to.py @@ -68,6 +68,9 @@ def _configure(self, robot, config): elif semantics.target_location.id in self._knowledge.location_names: know_target = True + elif semantics.target_location.type == "waypoint": + know_target = True + # # navigate to something else that we already know in the world # elif semantics.target_location.id and self._robot.ed.get_entity(semantics.target_location.id): # print "3" From 3d83c037065da8ef6771457489c406bf96ac7c15 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 15:51:07 -0400 Subject: [PATCH 31/71] fix(find): select person based on label --- action_server/src/action_server/actions/find.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index e48e6630..dce41e29 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -132,7 +132,7 @@ def _configure(self, robot, config): self._found_entity_designator = VariableDesignator(resolve_type=Entity) self._find_state_machines = [ states.FindPersonInRoom(robot, self._semantics.source_location.id, self._semantics.object.id, - False, self._found_entity_designator.writeable)] # This smach state should also return the found entity! + True, self._found_entity_designator.writeable)] self._config_result.context['location'] = { 'designator': EdEntityDesignator(self._robot, id=self._semantics.source_location.id) } From b30e9eda5a5cb67c21bb90100073bd4541d88fd5 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 15:51:37 -0400 Subject: [PATCH 32/71] fix(guide): use navigate state --- .../src/action_server/actions/guide.py | 35 ++++++++++++------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 8631d5da..1834a763 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -41,14 +41,14 @@ def _parse_semantics(semantics_dict): class Context: def __init__(self): - self.person = None + self.object = None @staticmethod def _parse_context(context_dict): context = Guide.Context() - if 'person' in context_dict: - context.person = resolve_entity_description(context_dict['person']) + if 'object' in context_dict: + context.object = resolve_entity_description(context_dict['object']) return context @@ -59,15 +59,16 @@ def _configure(self, robot, config): self._semantics = Guide._parse_semantics(config.semantics) self._context = Guide._parse_context(config.context) - person_known = False - if self._robot.ed.get_entity(id=self._semantics.object.id): - person_known = True - elif self._context.person: - person_known = True + person_designator = None + # if self._robot.ed.get_entity(id=self._semantics.object.id): + # person_designator = ds.EntityByIdDesignator(self._robot, id=self._semantics.object.id) + # elif self._context.object: + if self._context.object: + person_designator = self._context.object.designator # Before we start guiding, we need to know we're at the person we should guide # we check this by checking if we found a person earlier - if not person_known: + if not person_designator: self._config_result.required_context = { 'action': 'find', 'object': config.semantics['object'] @@ -78,13 +79,23 @@ def _configure(self, robot, config): target_location_designator = ds.EntityByIdDesignator(self._robot, id=self._semantics.target_location.id) - self._guide_state_machine = states.Guide(robot=self._robot, - target_location=target_location_designator, - follower=self._context.person.designator) + area = "near" + if self._semantics.target_location.type == "room": + area = "in" + + self._guide_state_machine = states.NavigateToSymbolic(robot=self._robot, + entity_designator_area_name_map={ + target_location_designator: area + }, + entity_lookat_designator=target_location_designator) self._config_result.succeeded = True def _start(self): + self._robot.head.look_at_standing_person() + self._robot.head.wait_for_motion_done() + self._robot.speech.speak("Follow me if you want to live") + self._robot.head.close() self._guide_state_machine.execute() self._execute_result.succeeded = True self._execute_result.message = "I guided " From 878056c9aa9e7fdd14723d20fbad70d24de700eb Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 17:34:30 -0400 Subject: [PATCH 33/71] fix(navigate_to): if person found, nav to waypoint --- .../src/action_server/actions/navigate_to.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py index 22eeb0e6..4abcf254 100644 --- a/action_server/src/action_server/actions/navigate_to.py +++ b/action_server/src/action_server/actions/navigate_to.py @@ -71,11 +71,6 @@ def _configure(self, robot, config): elif semantics.target_location.type == "waypoint": know_target = True - # # navigate to something else that we already know in the world - # elif semantics.target_location.id and self._robot.ed.get_entity(semantics.target_location.id): - # print "3" - # know_target = True - # navigate to 'it' elif semantics.target_location.type == 'reference' and context.object and \ context.object.id == semantics.target_location.id: @@ -85,8 +80,6 @@ def _configure(self, robot, config): semantics.target_location.id == context.object.id or semantics.target_location.id == "operator": know_target = True - # {'action': 'navigate-to', 'target-location': {'type': 'person', 'id': 'john', 'location': {'id': 'counter'}}} - if not know_target: # Request find action self._config_result.required_context = {'action': 'find', @@ -103,12 +96,12 @@ def _configure(self, robot, config): if semantics.target_location.id and \ (semantics.target_location.type != 'person' or semantics.target_location.id == 'operator'): entity_designator = EntityByIdDesignator(self._robot, id=semantics.target_location.id) - e = entity_designator.resolve() + e = entity_designator.resolve() # TODO: nasty assumption that we can resolve this entity here?! if e.is_a("waypoint"): self._navigation_state_machine = NavigateToWaypoint(self._robot, - waypoint_designator=entity_designator, - radius=0.1) + waypoint_designator=entity_designator, + radius=0.1) rospy.loginfo("Navigation set up for a waypoint") else: if e.is_a("room"): @@ -124,6 +117,11 @@ def _configure(self, robot, config): entity_designator_area_name_map={ entity_designator: area}, entity_lookat_designator=entity_designator) + elif semantics.target_location.type == 'person' and context.object: + self._navigation_state_machine = NavigateToWaypoint(self._robot, + waypoint_designator=context.object.designator, + radius=0.7) + rospy.loginfo("Navigation set up for a waypoint") else: entity_designator = context.object.designator self._navigation_state_machine = NavigateToSymbolic(self._robot, From 455d3467c1e4ec72a246be5040bb77eefd17dbc3 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 17:36:41 -0400 Subject: [PATCH 34/71] fix(navigate_to): fix nav to, assign to entity_designator --- action_server/src/action_server/actions/navigate_to.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py index 4abcf254..eab7b7fe 100644 --- a/action_server/src/action_server/actions/navigate_to.py +++ b/action_server/src/action_server/actions/navigate_to.py @@ -118,8 +118,9 @@ def _configure(self, robot, config): entity_designator: area}, entity_lookat_designator=entity_designator) elif semantics.target_location.type == 'person' and context.object: + entity_designator = context.object.designator self._navigation_state_machine = NavigateToWaypoint(self._robot, - waypoint_designator=context.object.designator, + waypoint_designator=entity_designator, radius=0.7) rospy.loginfo("Navigation set up for a waypoint") else: From 086eb959d99be90a97da60c3afdb9544a184b1be Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 17:38:50 -0400 Subject: [PATCH 35/71] feat(say): you shall not pass --- action_server/src/action_server/actions/say.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/action_server/src/action_server/actions/say.py b/action_server/src/action_server/actions/say.py index 9f4daad6..5aecee04 100644 --- a/action_server/src/action_server/actions/say.py +++ b/action_server/src/action_server/actions/say.py @@ -85,6 +85,9 @@ def _start(self): elif self._sentence == 'dark_side': line = " I'll never join you! " self._execute_result.message = " I told I'll never join the dark side. " + elif self._sentence == 'you_shall_not_pass': + line = " You shall not pass! " + self._execute_result.message = " I told them I won't let them pass. " elif self._sentence == 'joke': line = random.choice([ "What do you call a fish with no eyes? A fsh.", From db310c886fabeea2a3262761a7b79d180171c472 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 17:57:08 -0400 Subject: [PATCH 36/71] feat(say): start the party soundboard string --- action_server/src/action_server/actions/say.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/action_server/src/action_server/actions/say.py b/action_server/src/action_server/actions/say.py index 5aecee04..3d2c79c2 100644 --- a/action_server/src/action_server/actions/say.py +++ b/action_server/src/action_server/actions/say.py @@ -88,6 +88,9 @@ def _start(self): elif self._sentence == 'you_shall_not_pass': line = " You shall not pass! " self._execute_result.message = " I told them I won't let them pass. " + elif self._sentence == 'party': + line = "start the party" + self._execute_result.message = " I will get the party started. " elif self._sentence == 'joke': line = random.choice([ "What do you call a fish with no eyes? A fsh.", From 26feed11d5c862cbee77ca44b2231a464c7f2c57 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Tue, 19 Jun 2018 22:34:55 -0400 Subject: [PATCH 37/71] fix(find): not looking for specific persons in eegpsr --- action_server/src/action_server/actions/find.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index dce41e29..a34acb9a 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -132,7 +132,7 @@ def _configure(self, robot, config): self._found_entity_designator = VariableDesignator(resolve_type=Entity) self._find_state_machines = [ states.FindPersonInRoom(robot, self._semantics.source_location.id, self._semantics.object.id, - True, self._found_entity_designator.writeable)] + False, self._found_entity_designator.writeable)] self._config_result.context['location'] = { 'designator': EdEntityDesignator(self._robot, id=self._semantics.source_location.id) } From 97c620a28b19652b80cf8bb9179b41e8f9bcd2da Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 09:41:18 -0400 Subject: [PATCH 38/71] fix(say): better question for more info --- action_server/src/action_server/actions/say.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/say.py b/action_server/src/action_server/actions/say.py index 3d2c79c2..14d14764 100644 --- a/action_server/src/action_server/actions/say.py +++ b/action_server/src/action_server/actions/say.py @@ -13,7 +13,7 @@ class Say(Action): ''' def __init__(self): Action.__init__(self) - self._required_field_prompts = {'sentence' : "I didn't get what you want me to say."} + self._required_field_prompts = {'sentence': "What would you like me to say?"} self._required_skills = ['speech'] def _configure(self, robot, config): From 2d7daeea7db43bb3a92afdde329dd4f5de383d5e Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 09:42:21 -0400 Subject: [PATCH 39/71] refactor(follow): new semantics and context parsing, if no target, fine --- .../src/action_server/actions/follow.py | 56 +++++++++++++++---- 1 file changed, 44 insertions(+), 12 deletions(-) diff --git a/action_server/src/action_server/actions/follow.py b/action_server/src/action_server/actions/follow.py index 09f41dd9..a56e5dfa 100644 --- a/action_server/src/action_server/actions/follow.py +++ b/action_server/src/action_server/actions/follow.py @@ -1,6 +1,6 @@ from action import Action, ConfigurationData from find import Find -from entity_description import resolve_entity_description +from entity_description import resolve_entity_description, EntityDescription from robot_smach_states.navigation import NavigateToSymbolic from robot_smach_states.navigation import FollowOperator @@ -41,35 +41,67 @@ def __init__(self): self._required_field_prompts = {'target': " Who would you like me to follow? "} self._required_skills = ['base'] + class Semantics: + def __init__(self): + self.target = None + self.location_from = None + self.location_to = None + + @staticmethod + def _parse_semantics(semantics_dict): + semantics = Follow.Semantics() + + semantics.target = resolve_entity_description(semantics_dict['target']) + + if 'location-from' in semantics_dict: + semantics.location_from = resolve_entity_description(semantics_dict['location-from']) + + if 'location-to' in semantics_dict: + semantics.location_to = resolve_entity_description(semantics_dict['location-to']) + + return semantics + + class Context: + def __init__(self): + self.location = None + + @staticmethod + def _parse_context(context_dict): + context = Follow.Context() + + if 'location' in context_dict: + context.location = resolve_entity_description(context_dict['location']) + + return context + def _configure(self, robot, config): - self._target = resolve_entity_description(config.semantics["target"]) + semantics = Follow._parse_semantics(config.semantics) + context = Follow._parse_context(config.context) self._origin = None self._find_action = None + self._target = semantics.target - if self._target.type == "reference": + if not semantics.target or semantics.target.type == "reference": pass else: - if not self._target.id == "operator" and not "location-from" in config.semantics: + if not semantics.target.id == "operator" and not semantics.location_from: self._config_result.missing_field = "location-from" - self._config_result.message = " Where can I find {}? ".format(self._target.id) + self._config_result.message = " Where can I find {}? ".format(semantics.target.id) return - if "location-from" in config.semantics and "location-designator" not in config.context: + if semantics.location_from and not context.location: self._origin = resolve_entity_description(config.semantics["location-from"]) self._config_result.required_context = {'action': 'find', 'source-location': {'id': self._origin.id}, 'object': {'type': 'person', - 'id': self._target.id}} + 'id': semantics.target.id}} return - if self._target.id == "operator": + if semantics.target.id == "operator": self._target.id = "you" - if "location-to" in config.semantics: - self._goal = resolve_entity_description(config.semantics["location-to"]) - else: - self._goal = None + self._goal = semantics.location_to self._robot = robot From cb1291fe92d5a5f9aac1b91d4c6204149bace5ea Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 10:13:12 -0400 Subject: [PATCH 40/71] fix(guide): clearer msg when no target specified --- action_server/src/action_server/actions/guide.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 1834a763..08682689 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -14,7 +14,7 @@ class Guide(Action): ''' def __init__(self): Action.__init__(self) - self._required_field_prompts = {'object': " What exactly would you like me to find? "} + self._required_field_prompts = {'object': " Who would you like me to guide? "} self._required_skills = ['head', 'base', 'rightArm', 'speech'] self._follower_id = None self._target_id = None From 88c37fee174292eb406026b7352b52630fafd49a Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 10:13:40 -0400 Subject: [PATCH 41/71] fix(pick up): source location required field --- action_server/src/action_server/actions/pick_up.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/pick_up.py b/action_server/src/action_server/actions/pick_up.py index b338d292..bf3bd5b6 100644 --- a/action_server/src/action_server/actions/pick_up.py +++ b/action_server/src/action_server/actions/pick_up.py @@ -19,7 +19,8 @@ class PickUp(Action): def __init__(self): Action.__init__(self) self._required_skills = ['arms'] - self._required_field_prompts = {'object': " What would you like me to pick up? "} + self._required_field_prompts = {'object': " What would you like me to pick up? ", + 'source-location': " Where would you like me to pick that up? "} # TODO: handle source location from context? class Semantics: def __init__(self): From 0da360e857fb61ebb75e2a724bfb306a3fbab1a3 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 10:50:54 -0400 Subject: [PATCH 42/71] fix(pick up): object also found if category matches --- action_server/src/action_server/actions/pick_up.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/action_server/src/action_server/actions/pick_up.py b/action_server/src/action_server/actions/pick_up.py index bf3bd5b6..378c58cb 100644 --- a/action_server/src/action_server/actions/pick_up.py +++ b/action_server/src/action_server/actions/pick_up.py @@ -78,6 +78,8 @@ def _configure(self, robot, config): # Then the context object must be one of that category if self._knowledge.get_object_category(context.object.type) == semantics.object.category: object_found = True + if context.object.category == semantics.object.category: + object_found = True if not object_found: self._config_result.required_context = { From d6e4cc9ef603da6770c810d41587fbe2d187ea53 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 11:17:12 -0400 Subject: [PATCH 43/71] fix(server): wait for tc to get in position --- action_server/src/action_server/server.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/action_server/src/action_server/server.py b/action_server/src/action_server/server.py index de2e140c..011fb8d2 100644 --- a/action_server/src/action_server/server.py +++ b/action_server/src/action_server/server.py @@ -54,6 +54,9 @@ def _add_action_cb(self, goal): if configuration_result.succeeded: rospy.logdebug("Setting up state machine succeeded") + # TODO: remove this robocup hack! + rospy.loginfo("Waiting for TC to get in position...") + self._robot.speech.speak("Please step aside and let me get to work.", block=True) else: if configuration_result.missing_field: self._result.result = action_server_msgs.msg.TaskResult.RESULT_MISSING_INFORMATION From 61ea670b73f93678103dda8bd19c627d1a4994b7 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 11:52:32 -0400 Subject: [PATCH 44/71] fix(guide): target is required --- action_server/src/action_server/actions/guide.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 08682689..3222d3e6 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -14,7 +14,8 @@ class Guide(Action): ''' def __init__(self): Action.__init__(self) - self._required_field_prompts = {'object': " Who would you like me to guide? "} + self._required_field_prompts = {'object': " Who would you like me to guide? ", + 'target-location': " Where would you like me to guide them? "} self._required_skills = ['head', 'base', 'rightArm', 'speech'] self._follower_id = None self._target_id = None From cdfd4cc66d2f85952298e997baa461486a80eed5 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 13:09:09 -0400 Subject: [PATCH 45/71] fix(follow): sucessfully followed None --- action_server/src/action_server/actions/follow.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/follow.py b/action_server/src/action_server/actions/follow.py index a56e5dfa..61ee194c 100644 --- a/action_server/src/action_server/actions/follow.py +++ b/action_server/src/action_server/actions/follow.py @@ -124,7 +124,8 @@ def _start(self): self._execute_result.message += " But I failed to follow {} ".format(self._target.id) return self._robot.speech.speak("Thank you for guiding me.") - self._execute_result.message += " I successfully followed {} ".format(self._target.id) + self._execute_result.message += " I successfully followed {} ".format(self._target.id if self._target.id + else ".") if self._goal: self._execute_result.message += " to the {}".format(self._goal.id) From ecb697ab82614faf8627a8fc64f7e6805240fa10 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 04:16:36 +0200 Subject: [PATCH 46/71] feat(open door): add open door action --- .../src/action_server/actions/open_door.py | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 action_server/src/action_server/actions/open_door.py diff --git a/action_server/src/action_server/actions/open_door.py b/action_server/src/action_server/actions/open_door.py new file mode 100644 index 00000000..0fddf06c --- /dev/null +++ b/action_server/src/action_server/actions/open_door.py @@ -0,0 +1,100 @@ +from action import Action, ConfigurationData +from challenge_final import + + +class OpenDoor(Action): + """ + The Open Door action wraps the state machine to open the door of the cupboard at RWC 2018 + + """ + def __init__(self): + # Call the base class constructor + Action.__init__(self) + # self._required_field_prompts = {'object': " What object would you like me to work with? "} + + def _configure(self, robot, config): + self._state_machine = states.OpenDoor(robot) + self._config_result.succeeded = True + return + + def _start(self): + """ + Here, the action is actually performed. + """ + + """ + We typically run the state machine here. Under the hood, this state machine fills the entity designator. Because + this is the same object as the one in our _config_result.resulting_knowledge['object'], the context of this + entity is passed to any following states that used this Designator. + """ + result = self._action_fsm.execute() + + """ + After this, we may want to check the output of the state machine execution + """ + color = self._color_designator.resolve() + if result == 'not_available': + self._execute_result.message = " I haven't seen any %s object yet. " % color + self._execute_result.succeeded = False + return + + if result == 'error': + self._execute_result.message = " Something went wrong trying to determine which objects are %s " % color + self._execute_result.succeeded = False + return + + """ + Finally, we can return to the task manager that the action was performed successfully. Explicitly set succeeded + to True here, to let the TaskManager know that the Action succeeded. The message is reported to the operator + after the complete Task (series of Actions) is performed. + """ + self._execute_result.message = " I successfully performed the example action. " + self._execute_result.succeeded = True + return + + def _cancel(self): + """ + The _cancel method is called when the Action Server's goal is canceled and this action is still running + """ + + """ + # If the state machine is still running, we need to send it a preempt request + if self._action_fsm.is_running(): + self._action_fsm.request_preempt() + """ + +""" +The below code should not be in the Action implementation file. An action should always wrap an existing smach State +from the Robot Smach States. This is just here to be able to demonstrate the use in the above Action implementation. +""" +import smach + + +class DummyStateMachine(smach.State): + def __init__(self, robot, color_designator, entity_designator, object_colors): + """ + Select an entity with the given color. + :param robot: robot to execute state with + :param color_designator: A color that the resulting entity should have. + :param entity_designator: The resulting entity with the specified color + :param object_colors: Dict mapping from object type to color + """ + smach.State.__init__(self, outcomes=['succeeded', 'not_available', 'error']) + + # Assign member variables + self.robot = robot + self._color_designator = color_designator + self._entity_designator = entity_designator + self._object_colors = object_colors + + def execute(self, userdata=None): + try: + color = self._color_designator.resolve() + entities = self.robot.ed.get_entities() + for e in entities: + if self._object_colors[e.type] == color: + self._entity_designator.write(e) + return 'succeeded' + return 'not_available' + except: + return 'error' From 4bfc3fb6aea627d3c5a4bc837a04752a2cb8bd26 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 04:22:41 +0200 Subject: [PATCH 47/71] feat(clear table): add action --- .../src/action_server/actions/clear_table.py | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 action_server/src/action_server/actions/clear_table.py diff --git a/action_server/src/action_server/actions/clear_table.py b/action_server/src/action_server/actions/clear_table.py new file mode 100644 index 00000000..1d370c59 --- /dev/null +++ b/action_server/src/action_server/actions/clear_table.py @@ -0,0 +1,28 @@ +from action import Action, ConfigurationData +from challenge_dishwasher import NavigateAndPlaceInDishwasher, Grab, OpenDishwasher + +class ClearTable(Action): + def __init__(self): + Action.__init__(self) + + def _configure(self, robot, config): + self._state_machines = [OpenDishwasher(), Grab(), NavigateAndPlaceInDishwasher()] + + self._config_result.succeeded = True + self._active_state_machine = None + return + + def _start(self): + for sm in self._state_machines: + self._active_state_machine = sm + self._active_state_machine.execute() + + self._active_state_machine = None + + self._execute_result.message = " I cleaned the table! " + self._execute_result.succeeded = True + return + + def _cancel(self): + if self._active_state_machine: + self._active_state_machine.request_preempt() From e156223aa5c0da328d6260b67813e215ffa19ffc Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 20 Jun 2018 22:26:23 -0400 Subject: [PATCH 48/71] fix(server): remove robocup hack --- action_server/src/action_server/server.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/action_server/src/action_server/server.py b/action_server/src/action_server/server.py index 011fb8d2..de2e140c 100644 --- a/action_server/src/action_server/server.py +++ b/action_server/src/action_server/server.py @@ -54,9 +54,6 @@ def _add_action_cb(self, goal): if configuration_result.succeeded: rospy.logdebug("Setting up state machine succeeded") - # TODO: remove this robocup hack! - rospy.loginfo("Waiting for TC to get in position...") - self._robot.speech.speak("Please step aside and let me get to work.", block=True) else: if configuration_result.missing_field: self._result.result = action_server_msgs.msg.TaskResult.RESULT_MISSING_INFORMATION From 0d05ff76867984e0b09c232036cdebeaaf3d52fc Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 04:31:38 +0200 Subject: [PATCH 49/71] fix(init): add open door and clear table --- action_server/src/action_server/actions/__init__.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py index ff1e6bb4..ca3db2af 100644 --- a/action_server/src/action_server/actions/__init__.py +++ b/action_server/src/action_server/actions/__init__.py @@ -19,3 +19,6 @@ from send_picture import SendPicture from tell_name_of_person import TellNameOfPerson from turn_toward_sound import TurnTowardSound + +from clear_table import ClearTable +from open_door import OpenDoor From fc61eec516b86fa88e074649bdbe3e0bcfb49560 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 04:36:57 +0200 Subject: [PATCH 50/71] fix(open_door): add functionality of open door --- .../src/action_server/actions/open_door.py | 84 ++----------------- 1 file changed, 6 insertions(+), 78 deletions(-) diff --git a/action_server/src/action_server/actions/open_door.py b/action_server/src/action_server/actions/open_door.py index 0fddf06c..7d4914b2 100644 --- a/action_server/src/action_server/actions/open_door.py +++ b/action_server/src/action_server/actions/open_door.py @@ -1,10 +1,10 @@ from action import Action, ConfigurationData -from challenge_final import +from challenge_storing_groceries.open_door import OpenDoorMachine class OpenDoor(Action): """ - The Open Door action wraps the state machine to open the door of the cupboard at RWC 2018 + The Open Door action wraps the state machine to open the door of the cupboard at RWC 2018. """ def __init__(self): @@ -13,88 +13,16 @@ def __init__(self): # self._required_field_prompts = {'object': " What object would you like me to work with? "} def _configure(self, robot, config): - self._state_machine = states.OpenDoor(robot) + self._state_machine = OpenDoorMachine(robot) self._config_result.succeeded = True return def _start(self): - """ - Here, the action is actually performed. - """ + self._state_machine.execute() - """ - We typically run the state machine here. Under the hood, this state machine fills the entity designator. Because - this is the same object as the one in our _config_result.resulting_knowledge['object'], the context of this - entity is passed to any following states that used this Designator. - """ - result = self._action_fsm.execute() - - """ - After this, we may want to check the output of the state machine execution - """ - color = self._color_designator.resolve() - if result == 'not_available': - self._execute_result.message = " I haven't seen any %s object yet. " % color - self._execute_result.succeeded = False - return - - if result == 'error': - self._execute_result.message = " Something went wrong trying to determine which objects are %s " % color - self._execute_result.succeeded = False - return - - """ - Finally, we can return to the task manager that the action was performed successfully. Explicitly set succeeded - to True here, to let the TaskManager know that the Action succeeded. The message is reported to the operator - after the complete Task (series of Actions) is performed. - """ - self._execute_result.message = " I successfully performed the example action. " + self._execute_result.message = " I opened the cupboard door. " self._execute_result.succeeded = True return def _cancel(self): - """ - The _cancel method is called when the Action Server's goal is canceled and this action is still running - """ - - """ - # If the state machine is still running, we need to send it a preempt request - if self._action_fsm.is_running(): - self._action_fsm.request_preempt() - """ - -""" -The below code should not be in the Action implementation file. An action should always wrap an existing smach State -from the Robot Smach States. This is just here to be able to demonstrate the use in the above Action implementation. -""" -import smach - - -class DummyStateMachine(smach.State): - def __init__(self, robot, color_designator, entity_designator, object_colors): - """ - Select an entity with the given color. - :param robot: robot to execute state with - :param color_designator: A color that the resulting entity should have. - :param entity_designator: The resulting entity with the specified color - :param object_colors: Dict mapping from object type to color - """ - smach.State.__init__(self, outcomes=['succeeded', 'not_available', 'error']) - - # Assign member variables - self.robot = robot - self._color_designator = color_designator - self._entity_designator = entity_designator - self._object_colors = object_colors - - def execute(self, userdata=None): - try: - color = self._color_designator.resolve() - entities = self.robot.ed.get_entities() - for e in entities: - if self._object_colors[e.type] == color: - self._entity_designator.write(e) - return 'succeeded' - return 'not_available' - except: - return 'error' + pass From dd32a30983e93a9bf6bf5da9e0e62da8b4505322 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 20 Jun 2018 22:52:47 -0400 Subject: [PATCH 51/71] Correct imports --- action_server/src/action_server/actions/clear_table.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/action_server/src/action_server/actions/clear_table.py b/action_server/src/action_server/actions/clear_table.py index 1d370c59..8e39ac19 100644 --- a/action_server/src/action_server/actions/clear_table.py +++ b/action_server/src/action_server/actions/clear_table.py @@ -1,12 +1,12 @@ from action import Action, ConfigurationData -from challenge_dishwasher import NavigateAndPlaceInDishwasher, Grab, OpenDishwasher +from challenge_dishwasher.dishwasher import NavigateAndPlaceDishwasher, GrabRobust, OpenDishwasher class ClearTable(Action): def __init__(self): Action.__init__(self) def _configure(self, robot, config): - self._state_machines = [OpenDishwasher(), Grab(), NavigateAndPlaceInDishwasher()] + self._state_machines = [OpenDishwasher(), GrabRobust(), NavigateAndPlaceDishwasher()] self._config_result.succeeded = True self._active_state_machine = None From 61bc8e00569d52c71f7ef35662b4cca4e0595a48 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 20 Jun 2018 23:22:53 -0400 Subject: [PATCH 52/71] hardcoded cupboard in open_door action --- action_server/src/action_server/actions/open_door.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/open_door.py b/action_server/src/action_server/actions/open_door.py index 7d4914b2..e2703c79 100644 --- a/action_server/src/action_server/actions/open_door.py +++ b/action_server/src/action_server/actions/open_door.py @@ -13,7 +13,7 @@ def __init__(self): # self._required_field_prompts = {'object': " What object would you like me to work with? "} def _configure(self, robot, config): - self._state_machine = OpenDoorMachine(robot) + self._state_machine = OpenDoorMachine(robot, "cupboard", "in_front_of", "on_top_of") self._config_result.succeeded = True return From e6951deb55adda6f6d9777dad1fbf7e69fb0b8db Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 20 Jun 2018 23:31:36 -0400 Subject: [PATCH 53/71] fixed sm imports --- action_server/src/action_server/actions/open_door.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/open_door.py b/action_server/src/action_server/actions/open_door.py index 7d4914b2..d02302c6 100644 --- a/action_server/src/action_server/actions/open_door.py +++ b/action_server/src/action_server/actions/open_door.py @@ -13,7 +13,7 @@ def __init__(self): # self._required_field_prompts = {'object': " What object would you like me to work with? "} def _configure(self, robot, config): - self._state_machine = OpenDoorMachine(robot) + self._state_machine = OpenDoorMachine(robot, "cupboard", "in_front_of", "shelf6") self._config_result.succeeded = True return From 0688eec71916a6bfbd5dc40661c45761f1bb40b6 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 20 Jun 2018 23:48:01 -0400 Subject: [PATCH 54/71] constructor fixes --- action_server/src/action_server/actions/clear_table.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/clear_table.py b/action_server/src/action_server/actions/clear_table.py index 8e39ac19..687129d9 100644 --- a/action_server/src/action_server/actions/clear_table.py +++ b/action_server/src/action_server/actions/clear_table.py @@ -6,7 +6,7 @@ def __init__(self): Action.__init__(self) def _configure(self, robot, config): - self._state_machines = [OpenDishwasher(), GrabRobust(), NavigateAndPlaceDishwasher()] + self._state_machines = [OpenDishwasher(robot, "dishwasher"), GrabRobust(robot), NavigateAndPlaceDishwasher(robot)] self._config_result.succeeded = True self._active_state_machine = None From ba5e2647282d2db6aff27efa151a44710dfde010 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 05:56:31 +0200 Subject: [PATCH 55/71] fix(guide final challenge): hard coded guide state. Hacky hacky! --- .../src/action_server/actions/__init__.py | 1 + .../actions/guide_final_challenge.py | 73 +++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 action_server/src/action_server/actions/guide_final_challenge.py diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py index ca3db2af..454aa416 100644 --- a/action_server/src/action_server/actions/__init__.py +++ b/action_server/src/action_server/actions/__init__.py @@ -22,3 +22,4 @@ from clear_table import ClearTable from open_door import OpenDoor +from guide_final_challenge import GuideFinalChallenge diff --git a/action_server/src/action_server/actions/guide_final_challenge.py b/action_server/src/action_server/actions/guide_final_challenge.py new file mode 100644 index 00000000..39456339 --- /dev/null +++ b/action_server/src/action_server/actions/guide_final_challenge.py @@ -0,0 +1,73 @@ +from action import Action, ConfigurationData +import robot_smach_states as states +import robot_smach_states.util.designators as ds +from entity_description import resolve_entity_description + +import rospy + + +class GuideFinalChallenge(Action): + ''' The GuideFinalChallenge. class navigates to a target, telling someone to follow the robot and about arriving at the target. + + Parameters to pass to the configure() method are: + - `object` (required): the id of the entity to navigate to + ''' + def __init__(self): + Action.__init__(self) + self._required_field_prompts = {'object': " Who would you like me to guide? ", + 'target-location': " Where would you like me to guide them? "} + self._required_skills = ['head', 'base', 'rightArm', 'speech'] + self._follower_id = None + self._target_id = None + + class Semantics: + def __init__(self): + self.object = None + self.source_location = None + self.target_location = None + + @staticmethod + def _parse_semantics(semantics_dict): + semantics = GuideFinalChallenge.Semantics() + + semantics.object = resolve_entity_description(semantics_dict['object']) + + if 'source-location' in semantics_dict: + semantics.source_location = resolve_entity_description(semantics_dict['source-location']) + + if 'target-location' in semantics_dict: + semantics.target_location = resolve_entity_description(semantics_dict['target-location']) + + return semantics + + def _configure(self, robot, config): + self._robot = robot + + # We start by parsing semantics and context + self._semantics = GuideFinalChallenge._parse_semantics(config.semantics) + + target_location_designator = ds.EntityByIdDesignator(self._robot, id=self._semantics.target_location.id) + + area = "near" + if self._semantics.target_location.type == "room": + area = "in" + + self._state_machine = states.NavigateToSymbolic(robot=self._robot, + entity_designator_area_name_map={ + target_location_designator: area + }, + entity_lookat_designator=target_location_designator) + + self._config_result.succeeded = True + + def _start(self): + self._robot.head.look_at_standing_person() + self._robot.head.wait_for_motion_done() + self._robot.speech.speak("Follow me. I will guide you to the {}".format(self._semantics.target_location.id)) + self._robot.head.close() + self._state_machine.execute() + self._execute_result.succeeded = True + self._execute_result.message = " Here you go! " + + def _cancel(self): + self._state_machine.request_preempt() From 004250e86b0b995ff89ef5c0afdc5ff7bafdb720 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 06:03:45 +0200 Subject: [PATCH 56/71] fix(guide final): speak after guide --- .../src/action_server/actions/guide_final_challenge.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/guide_final_challenge.py b/action_server/src/action_server/actions/guide_final_challenge.py index 39456339..52885be7 100644 --- a/action_server/src/action_server/actions/guide_final_challenge.py +++ b/action_server/src/action_server/actions/guide_final_challenge.py @@ -67,7 +67,8 @@ def _start(self): self._robot.head.close() self._state_machine.execute() self._execute_result.succeeded = True - self._execute_result.message = " Here you go! " + self._execute_result.message = " I guided! " + self._robot.speech.speak("Here you go!") def _cancel(self): self._state_machine.request_preempt() From 2e89756e3f0d482f864638cf50c342615c9eee4f Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Thu, 21 Jun 2018 00:35:23 -0400 Subject: [PATCH 57/71] fix(clear_table): first navigate, then open dishwasher --- action_server/src/action_server/actions/clear_table.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/action_server/src/action_server/actions/clear_table.py b/action_server/src/action_server/actions/clear_table.py index 687129d9..e5159e7a 100644 --- a/action_server/src/action_server/actions/clear_table.py +++ b/action_server/src/action_server/actions/clear_table.py @@ -1,12 +1,12 @@ from action import Action, ConfigurationData -from challenge_dishwasher.dishwasher import NavigateAndPlaceDishwasher, GrabRobust, OpenDishwasher +from challenge_dishwasher.dishwasher import NavigateAndPlaceDishwasher, GrabRobust, NavigateAndOpenDishwasher class ClearTable(Action): def __init__(self): Action.__init__(self) def _configure(self, robot, config): - self._state_machines = [OpenDishwasher(robot, "dishwasher"), GrabRobust(robot), NavigateAndPlaceDishwasher(robot)] + self._state_machines = [GrabRobust(robot), NavigateAndOpenDishwasher(robot), NavigateAndPlaceDishwasher(robot)] self._config_result.succeeded = True self._active_state_machine = None From 688224cd2773cecfc457ff4da85456168bfec37e Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 21 Jun 2018 09:47:58 -0400 Subject: [PATCH 58/71] feat(find): if find person, select on label --- action_server/src/action_server/actions/find.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index a34acb9a..dce41e29 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -132,7 +132,7 @@ def _configure(self, robot, config): self._found_entity_designator = VariableDesignator(resolve_type=Entity) self._find_state_machines = [ states.FindPersonInRoom(robot, self._semantics.source_location.id, self._semantics.object.id, - False, self._found_entity_designator.writeable)] + True, self._found_entity_designator.writeable)] self._config_result.context['location'] = { 'designator': EdEntityDesignator(self._robot, id=self._semantics.source_location.id) } From dccac75a00fa08742f1e25f92b6717dd3a1a9769 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 26 Sep 2018 13:48:58 +0200 Subject: [PATCH 59/71] remove double dep --- action_server/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/action_server/package.xml b/action_server/package.xml index 75b2307d..bd8c442c 100644 --- a/action_server/package.xml +++ b/action_server/package.xml @@ -15,5 +15,4 @@ robot_skills actionlib challenge_presentation - challenge_presentation From d56823b45abc240d2f560c260b2ccd1dd0c2ce4d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 18 Nov 2018 21:46:16 +0100 Subject: [PATCH 60/71] add dep on robot_smach_states --- action_server/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/action_server/package.xml b/action_server/package.xml index bd8c442c..f2b2d956 100644 --- a/action_server/package.xml +++ b/action_server/package.xml @@ -13,6 +13,7 @@ action_server_msgs grammar_parser robot_skills + robot_smach_states actionlib challenge_presentation From 10483261f1765331e000ad17bfac061fbfd8a02a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 19 Nov 2018 14:25:00 +0100 Subject: [PATCH 61/71] convert recipe to lowercase --- action_server/src/action_server/server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/server.py b/action_server/src/action_server/server.py index de2e140c..1ab2e5b9 100644 --- a/action_server/src/action_server/server.py +++ b/action_server/src/action_server/server.py @@ -35,7 +35,7 @@ def _get_actions_cb(self, req): return res def _add_action_cb(self, goal): - recipe = yaml.load(goal.recipe) + recipe = yaml.load(goal.recipe.lower()) rospy.logdebug("Received action recipe: {}".format(goal.recipe)) try: From 3d55154778cd975ebd9948ed4c61b7793c829ebf Mon Sep 17 00:00:00 2001 From: Lars Janssen Date: Tue, 12 Feb 2019 20:24:35 +0100 Subject: [PATCH 62/71] Update docstring --- action_server/src/action_server/actions/count_and_tell.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index 067469ab..fd7d81dc 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -8,11 +8,13 @@ class CountAndTell(Action): - ''' The CountAndTell class implements the action to count the number of objects and tell the operator. + """ + The CountAndTell class implements the action to count the number of objects in a specific location and tell the + operator the amount of objects, the type and the location. Parameters to pass to the configure() method are: - - `sentence` (required): The sentence to speak. May be a keyword to tell something more intelligent. - ''' + - `config` (required): the ConfigurationData defines the input data structure for configuration of an action. + """ def __init__(self): Action.__init__(self) self._required_skills = ['speech'] From d72d58e2e7d0ba4b8e8a0f36f1b1bd1b76844e82 Mon Sep 17 00:00:00 2001 From: Lars Janssen Date: Tue, 12 Feb 2019 20:27:13 +0100 Subject: [PATCH 63/71] Remove commented code --- action_server/src/action_server/actions/guide.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 3222d3e6..09de677a 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -61,9 +61,6 @@ def _configure(self, robot, config): self._context = Guide._parse_context(config.context) person_designator = None - # if self._robot.ed.get_entity(id=self._semantics.object.id): - # person_designator = ds.EntityByIdDesignator(self._robot, id=self._semantics.object.id) - # elif self._context.object: if self._context.object: person_designator = self._context.object.designator From 8446ebf6e18b04314cfe4b5a30137ce627060067 Mon Sep 17 00:00:00 2001 From: Lars Janssen Date: Tue, 26 Feb 2019 17:59:24 +0100 Subject: [PATCH 64/71] Remove RWC2018 specific naming --- action_server/src/action_server/actions/tell_name_of_person.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/tell_name_of_person.py b/action_server/src/action_server/actions/tell_name_of_person.py index 6c442230..faf4edde 100644 --- a/action_server/src/action_server/actions/tell_name_of_person.py +++ b/action_server/src/action_server/actions/tell_name_of_person.py @@ -96,7 +96,7 @@ def _start(self): if tries < 2: self._robot.speech.speak("Sorry, I did not understand you, try again.") else: - self._robot.speech.speak("Sorry, I was unable to understand you again. I'll just name you Maple. ") + self._robot.speech.speak("Sorry, I was unable to understand you again. I'll just name you Max. ") self._execute_result.message = " I did not understand the answer. " tries += 1 From 1ac2563ab0f55990e0296995150e7a1eb8badcd7 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 9 Mar 2019 12:23:14 +0100 Subject: [PATCH 65/71] Removed the clear_table action since it is a very ugly implementation. --- action_server/src/action_server/actions/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py index 454aa416..7eb82a02 100644 --- a/action_server/src/action_server/actions/__init__.py +++ b/action_server/src/action_server/actions/__init__.py @@ -20,6 +20,5 @@ from tell_name_of_person import TellNameOfPerson from turn_toward_sound import TurnTowardSound -from clear_table import ClearTable from open_door import OpenDoor from guide_final_challenge import GuideFinalChallenge From c0144a677afa4b4720c9b4618b69d67b33c77ac7 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 19 Mar 2019 21:33:57 +0100 Subject: [PATCH 66/71] Rename inspect.py to inspect_action.py, to avoid conflicts with the 'inspect' builtin Python module --- action_server/src/action_server/actions/__init__.py | 2 +- action_server/src/action_server/actions/guide.py | 5 +++-- .../action_server/actions/{inspect.py => inspect_action.py} | 0 3 files changed, 4 insertions(+), 3 deletions(-) rename action_server/src/action_server/actions/{inspect.py => inspect_action.py} (100%) diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py index 7eb82a02..b3e3c5a3 100644 --- a/action_server/src/action_server/actions/__init__.py +++ b/action_server/src/action_server/actions/__init__.py @@ -9,7 +9,7 @@ from guide import Guide from gripper_goal import GripperGoal from hand_over import HandOver -from inspect import Inspect +from inspect_action import Inspect from look_at import LookAt from navigate_to import NavigateTo from pick_up import PickUp diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 09de677a..02beb450 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -1,10 +1,11 @@ +import rospy + from action import Action, ConfigurationData + import robot_smach_states as states import robot_smach_states.util.designators as ds from entity_description import resolve_entity_description -import rospy - class Guide(Action): ''' The Guide class navigates to a target, telling someone to follow the robot and about arriving at the target. diff --git a/action_server/src/action_server/actions/inspect.py b/action_server/src/action_server/actions/inspect_action.py similarity index 100% rename from action_server/src/action_server/actions/inspect.py rename to action_server/src/action_server/actions/inspect_action.py From f67e837ee43a0dc186e768eb35180c2a40ac649c Mon Sep 17 00:00:00 2001 From: Lars Janssen Date: Tue, 19 Mar 2019 21:59:59 +0100 Subject: [PATCH 67/71] add hero as a robot --- action_server/src/action_server/main.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/main.py b/action_server/src/action_server/main.py index 92eed7af..91a474cb 100755 --- a/action_server/src/action_server/main.py +++ b/action_server/src/action_server/main.py @@ -23,8 +23,10 @@ from robot_skills.amigo import Amigo as Robot elif robot_name == 'sergio': from robot_skills.sergio import Sergio as Robot + elif robot_name == 'hero': + from robot_skills.hero import Hero as Robot else: - rospy.logerr("'robot_name' must be either 'amigo' or 'sergio'") + rospy.logerr("'robot_name' must be either 'amigo', 'hero, or 'sergio'") sys.exit() robot = Robot() From 9131e7cdd9b90b7daf0c8dc0160e027401deb3bb Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 26 Mar 2019 22:46:06 +0100 Subject: [PATCH 68/71] completed __main__ in guide.py --- action_server/src/action_server/actions/guide.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py index 02beb450..17c4ccfb 100644 --- a/action_server/src/action_server/actions/guide.py +++ b/action_server/src/action_server/actions/guide.py @@ -112,6 +112,8 @@ def _cancel(self): from robot_skills.amigo import Amigo as Robot elif robot_name == 'sergio': from robot_skills.sergio import Sergio as Robot + elif robot_name == 'hero': + from robot_skills.hero import Hero as Robot else: from robot_skills.mockbot import Mockbot as Robot @@ -120,7 +122,8 @@ def _cancel(self): action = Guide() config = ConfigurationData({'action': 'guide', - 'object': {'id': 'cabinet'}}) + 'object': {'id': 'cabinet'}, + 'target-location': {'id':'dinner_table'}}) action.configure(robot, config) action.start() From 8715110eb3bd4b94abc418c060245d2866aac839 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 2 Apr 2019 23:08:25 +0200 Subject: [PATCH 69/71] find smach state is called differently when looking for anyone --- action_server/src/action_server/actions/find.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py index dce41e29..c94a75fe 100644 --- a/action_server/src/action_server/actions/find.py +++ b/action_server/src/action_server/actions/find.py @@ -129,10 +129,13 @@ def _configure(self, robot, config): # person in room if not self._semantics.object.id: self._semantics.object.id = 'someone' + discard_other_labels = False + else: + discard_other_labels = True self._found_entity_designator = VariableDesignator(resolve_type=Entity) self._find_state_machines = [ states.FindPersonInRoom(robot, self._semantics.source_location.id, self._semantics.object.id, - True, self._found_entity_designator.writeable)] + discard_other_labels, self._found_entity_designator.writeable)] self._config_result.context['location'] = { 'designator': EdEntityDesignator(self._robot, id=self._semantics.source_location.id) } From 37ac1d20fbf5f42e5a4299c5d06ca58cd4520ff0 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 9 Apr 2019 20:39:22 +0200 Subject: [PATCH 70/71] TEMP - simple temp fix to allow further development --- action_server/src/action_server/actions/__init__.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py index b3e3c5a3..d5c89442 100644 --- a/action_server/src/action_server/actions/__init__.py +++ b/action_server/src/action_server/actions/__init__.py @@ -20,5 +20,4 @@ from tell_name_of_person import TellNameOfPerson from turn_toward_sound import TurnTowardSound -from open_door import OpenDoor -from guide_final_challenge import GuideFinalChallenge +# from open_door import OpenDoor From 72449736f428ff30ab4d3d867a2c66a223b829e8 Mon Sep 17 00:00:00 2001 From: Lars Janssen Date: Tue, 23 Apr 2019 19:24:23 +0200 Subject: [PATCH 71/71] Clean up(count and tell):fix docstring --- action_server/src/action_server/actions/count_and_tell.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/action_server/src/action_server/actions/count_and_tell.py b/action_server/src/action_server/actions/count_and_tell.py index fd7d81dc..b282db7e 100644 --- a/action_server/src/action_server/actions/count_and_tell.py +++ b/action_server/src/action_server/actions/count_and_tell.py @@ -13,7 +13,8 @@ class CountAndTell(Action): operator the amount of objects, the type and the location. Parameters to pass to the configure() method are: - - `config` (required): the ConfigurationData defines the input data structure for configuration of an action. + - `config` (required): the ConfigurationData defines the input data structure for configuration of an action i.e + a JSON string with for this action a "location" and an " object". """ def __init__(self): Action.__init__(self) @@ -58,7 +59,7 @@ def _start(self): self._execute_result.succeeded = False self._execute_result.message = "I failed to count the objects." return - + self._execute_result.succeeded = True # This message is instantiated in _configure but leaves some stuff to be formatted