def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
states.Say(robot,
"Starting navigation test",
block=False),
transitions={'spoken': 'SAY_TEST_SYMBOLIC'})
smach.StateMachine.add('SAY_TEST_SYMBOLIC',
states.Say(robot,
"Testing Navigate To Symbolic",
block=False),
transitions={'spoken': 'NAV_TO_SYMB'})
smach.StateMachine.add(
'NAV_TO_SYMB',
states.NavigateToSymbolic(
robot,
{EntityByIdDesignator(robot, id="living_room"): "in"},
EntityByIdDesignator(robot, id="dinnertable")),
transitions={
'arrived': 'SAY_TEST_WAYPOINT',
'unreachable': 'SAY_FAILED_SYMBOLIC',
'goal_not_defined': 'SAY_FAILED_SYMBOLIC'
})
smach.StateMachine.add('SAY_FAILED_SYMBOLIC',
states.Say(robot,
"Failed Navigate To Symbolic.",
block=True),
transitions={'spoken': 'SAY_TEST_WAYPOINT'})
smach.StateMachine.add('SAY_TEST_WAYPOINT',
states.Say(robot,
"Testing Navigate To Waypoint.",
block=False),
transitions={'spoken': 'NAV_TO_WAYPOINT'})
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'container_success',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add('SAY_FAILED_WAYPOINT',
states.Say(robot,
"Failed reaching the waypoint.",
block=True),
transitions={'spoken': 'container_success'})
def __init__(self, robot, found_entity_classifications_designator, known_types, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done'])
selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', )
with self:
smach.StateMachine.add("SELECT_ENTITY",
SelectEntity(robot, found_entity_classifications_designator,
selected_entity_designator),
transitions={"entity_selected": "DETERMINE_ACTION",
"no_entities_left": "done"})
smach.StateMachine.add("DETERMINE_ACTION",
DetermineAction(robot, selected_entity_designator, known_types),
transitions={"self": "SELF_CLEANUP",
"operator": "OPERATOR_CLEANUP",
"failed": "SELECT_ENTITY",
"other_robot": "OTHER_ROBOT_CLEANUP"})
smach.StateMachine.add("SELF_CLEANUP", SelfCleanup(robot, selected_entity_designator, location_id, segment_area),
transitions={"done": "SELECT_ENTITY", "failed": "SELECT_ENTITY"})
smach.StateMachine.add("OPERATOR_CLEANUP", OperatorCleanup(robot, selected_entity_designator, location_id, segment_area),
transitions={"cleanup": "SELF_CLEANUP", "no_cleanup": "SELECT_ENTITY"})
smach.StateMachine.add("OTHER_ROBOT_CLEANUP", OtherRobotCleanup(robot, selected_entity_designator, location_id, segment_area),
transitions={"done": "SELECT_ENTITY"})
def __init__(self, robot, found_entity_classifications_designator,
location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done'])
selected_entity_designator = EntityByIdDesignator(
robot,
"TBD",
name='selected_entity_designator',
)
with self:
smach.StateMachine.add(
"SELECT_ENTITY",
SelectEntity(robot, found_entity_classifications_designator,
selected_entity_designator),
transitions={
"entity_selected": "SELF_CLEANUP",
"no_entities_left": "done"
})
smach.StateMachine.add("SELF_CLEANUP",
SelfCleanup(robot,
selected_entity_designator,
location_id, segment_area),
transitions={
"done": "SELECT_ENTITY",
"failed": "SELECT_ENTITY"
})
def execute(self, userdata=None):
# Try to pop item from entities_ids_designator
correct_entity = False
while not correct_entity:
try:
entity_classification = self._entity_classifications_designator.resolve(
).pop()
except Exception:
return "no_entities_left"
entity = EntityByIdDesignator(
self._robot,
entity_classification.id).resolve() # type: Entity
shape = entity.shape
size_x = max(shape.x_max - shape.x_min, 0.001)
size_y = max(shape.y_max - shape.y_min, 0.001)
if size_x > self._xy_limit or size_y > self._xy_limit:
continue
if not 1 / min(self._xy_ratio, 1000) <= size_x / size_y <= min(
self._xy_ratio, 1000):
continue
correct_entity = True
rospy.loginfo("We have selected the entity with id %s" %
entity_classification.id)
self._selected_entity_designator.id_ = entity_classification.id
return "entity_selected"
def __init__(self, robot, selected_entity_designator, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done'])
sentences = ["%s, please clean this object %s the %s" % (other_robot_name, segment_area, location_id),
"%s, can you clean the trash %s the %s?" % (other_robot_name, segment_area, location_id),
"Can another robot clean the garbage %s the %s?" % (segment_area, location_id)]
with self:
smach.StateMachine.add('SAY_OTHER_ROBOT_CLEANUP',
robot_smach_states.Say(robot, sentences, block=False),
transitions={"spoken": "NAVIGATE_TO_INITIAL"})
smach.StateMachine.add( "NAVIGATE_TO_INITIAL",
robot_smach_states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_point,), radius=0.05),
transitions={ 'arrived' : 'CONTACT_OTHER_ROBOT',
'unreachable' : 'CONTACT_OTHER_ROBOT',
'goal_not_defined' : 'CONTACT_OTHER_ROBOT'})
smach.StateMachine.add('CONTACT_OTHER_ROBOT',
ContactOtherRobot(robot, selected_entity_designator),
transitions={"done": "WAIT_FOR_TRIGGER", "failed": "SAY_FAILED"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(robot, ["continue", "gpsr"], "/amigo/trigger"),
transitions={"continue": "SAY_DONE", "gpsr": "SAY_FAILED", "preempted" : "SAY_FAILED"})
smach.StateMachine.add('SAY_DONE',
robot_smach_states.Say(robot, ["Thanks for cleaning", "Thank you", "You are the best"], block=True),
transitions={"spoken": "done"})
smach.StateMachine.add('SAY_FAILED',
robot_smach_states.Say(robot, ["Too bad then, we will just leave that trash there"], block=True),
transitions={"spoken": "done"})
def setup_statemachine(robot):
cabinet = EntityByIdDesignator(robot, id="bookcase", name="pick_shelf")
ds = EmptyShelfDesignator(robot, cabinet, name="placement", area=PLACE_SHELF)
for i in range(5):
ds.resolve()
import ipdb;ipdb.set_trace()
def task_finished(self, message):
# Navigate to the GPSR meeting point
self.finished = rospy.get_time() - self.start_time > (
60 * self.time_limit - 45) and self.tasks_done >= 1
if not self.skip and not self.finished:
self.robot.speech.speak("Moving to the meeting point.",
block=False)
nwc = NavigateToWaypoint(robot=self.robot,
waypoint_designator=EntityByIdDesignator(
robot=self.robot,
id=self.knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
self.robot.head.look_at_standing_person()
self._say_to_user(message)
self._say_to_user(
"I performed {} {} so far, still going strong!".format(
self.tasks_done,
"tasks" if self.tasks_done == 1 else "tasks"))
self.timeout_count = 0
self._start_wait_for_command(self.knowledge.grammar,
self.knowledge.grammar_target)
else:
rospy.logwarn(
"Not going to meeting point, challenge has param skip:=true set"
)
if (self.tasks_done >= self.tasks_to_be_done
or self.finished) and not self.skip:
nwc = NavigateToWaypoint(robot=self.robot,
waypoint_designator=EntityByIdDesignator(
robot=self.robot,
id=self.knowledge.exit_waypoint),
radius=0.3)
self.robot.speech.speak(
"I'm done now. Thank you very much, and goodbye!", block=True)
nwc.execute()
return
def test_delivery(robot):
from robot_skills.util.kdl_conversions import kdl_frame_stamped_from_XYZRPY
robot.ed.update_entity(id="one",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=1.0, y=0, frame_id="/map"),
type="waypoint")
robot.ed.update_entity(id="two",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=-1.2, y=0.0, frame_id="/map"),
type="waypoint")
robot.ed.update_entity(id="three",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=1.950, y=1.551, frame_id="/map"),
type="waypoint")
global ORDERS
ORDERS = {
"beverage": {
"name": "coke",
"location": "one"
},
"combo": {
"name": "pringles and chocolate",
"location": "two"
}
}
deliver = smach.StateMachine(outcomes=['done', 'aborted'])
with deliver:
smach.StateMachine.add('DELIVER_BEVERAGE',
DeliverOrderWithBasket(robot, "beverage"),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'NAVIGATE_TO_KITCHEN'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot, id="kitchen"),
radius=WAYPOINT_RADIUS),
transitions={
'arrived': 'DELIVER_COMBO',
'unreachable': 'DELIVER_COMBO',
'goal_not_defined': 'DELIVER_COMBO'
})
smach.StateMachine.add('DELIVER_COMBO',
DeliverOrderWithBasket(robot, "combo"),
transitions={
'succeeded': 'done',
'failed': 'aborted'
})
deliver.execute(None)
def execute(self, userdata=None):
ids_to_select_from = [e.id for e in self._candidate_entities_designator.resolve()]
rospy.logdebug('list of entities to select from: {}'.format(ids_to_select_from))
satisfying_entities = []
for id in ids_to_select_from:
e = EntityByIdDesignator(self._robot, id).resolve()
object_height = e.shape.z_max - e.shape.z_min
rospy.logerr("Id: {}, height: {}".format(id, object_height))
if 0.02 < object_height < 0.15:
satisfying_entities.append(e)
rospy.logdebug('entities that satisfy the seleciton criteria: {}'.format(satisfying_entities))
if satisfying_entities:
rospy.logerr("Cup Id: {}".format(satisfying_entities[0].id))
self._found_entity_designator.write(satisfying_entities[0])
return 'succeeded'
else:
return 'failed'
def main():
rospy.init_node("ee_gpsr")
parser = argparse.ArgumentParser()
parser.add_argument('robot', help='Robot name')
parser.add_argument('--once',
action='store_true',
help='Turn on infinite loop')
parser.add_argument('--skip', action='store_true', help='Skip enter/exit')
parser.add_argument('sentence', nargs='*', help='Optional sentence')
args = parser.parse_args()
rospy.loginfo('args: %s', args)
mock_sentence = " ".join(
[word for word in args.sentence if word[0] != '_'])
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if args.robot == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif args.robot == 'sergio':
from robot_skills.sergio import Sergio as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
# Sleep for 1 second to make sure everything is connected
time.sleep(1)
command_center = CommandCenter(robot)
challenge_knowledge = load_knowledge('challenge_eegpsr')
command_center.set_grammar(
os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not args.skip:
# Wait for door, enter arena
s = StartChallengeRobust(robot, challenge_knowledge.initial_pose)
s.execute()
# Move to the start location
nwc = NavigateToWaypoint(robot,
EntityByIdDesignator(
robot,
id=challenge_knowledge.starting_pose),
radius=0.3)
nwc.execute()
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
sentence = " ".join([word for word in args.sentence if word[0] != '_'])
first = True
if sentence:
semantics = command_center.parse_command(sentence)
if not semantics:
rospy.logerr("Cannot parse \"{}\"".format(sentence))
return
command_center.execute_command(semantics)
else:
while True:
if first:
# First sentence robot says
sentences = [
"Hello there! Welcome to the double E GPSR. You can give me an order, but wait for the ping."
]
else:
# Sentence robot says after completing a task
sentences = [
"Hello there, you look lovely! I'm here to take a new order, but wait for the ping!"
]
# These sentences are for when the first try fails
# (Robot says "Do you want me to ...?", you say "No", then robot says sentence below)
sentences += [
"I'm so sorry! Can you please speak louder and slower? And wait for the ping!",
"Again, I am deeply sorry. Bad robot! Please try again, but wait for the ping!",
"You and I have communication issues. Speak up! Tell me what you want, but wait for the ping"
]
hey_robot_wait_forever(robot)
res = command_center.request_command(ask_confirmation=True,
ask_missing_info=False,
timeout=600,
sentences=sentences)
if not res:
continue
first = False
(sentence, semantics) = res
if not command_center.execute_command(semantics):
robot.speech.speak("I am truly sorry, let's try this again")
if args.once:
break
if not args.skip:
nwc = NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.starting_pose),
radius=0.3)
nwc.execute()
def __init__(self, robot, source_location, source_navArea, target_location, target_navArea, target_placeArea="on_top_of", source_searchArea="on_top_of"):
"""
Let the given robot move to a location and remove all entities from that table one at a time
:param robot: Robot to use
:param source_location: Location which will be cleared
:param target_location: Location where the objects will be placed
:return:
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
# Check types or designator resolve types
#check_type(source_location, Entity)
#check_type(target_location, Entity)
segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult])
selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', )
arm_des = UnoccupiedArmDesignator(robot, {}).lockable()
arm_des.lock()
place_position = states.util.designators.EmptySpotDesignator(robot, EdEntityDesignator(
robot, id=target_location.id),
area="on_top_of"
)
with self:
smach.StateMachine.add('INSPECT_SOURCE_ENTITY',
states.Inspect(robot, source_location, objectIDsDes=segmented_entities_designator,
searchArea=source_searchArea, navigation_area=source_navArea),
transitions={'done': 'DETERMINE_IF_CLEAR',
'failed': 'failed'}
)
#smach.StateMachine.add('DETERMINE_IF_CLEAR',
# isitclear(robot=robot,
# objectIDsDes=segmented_entities_designator),
# transitions={'clear': 'done',
# 'not_clear': 'failed'})
smach.StateMachine.add('DETERMINE_IF_CLEAR',
SelectEntity(robot, segmented_entities_designator, selected_entity_designator),
transitions={'no_entities_left': 'done',
'entity_selected': 'GRAB'}
)
smach.StateMachine.add('GRAB',
states.Grab(robot, selected_entity_designator, arm_des),
transitions={'done': 'INSPECT_TARGET',
'failed': 'failed'}
)
smach.StateMachine.add('INSPECT_TARGET',
states.Inspect(robot, target_location, searchArea=target_placeArea,
navigation_area=target_navArea),
transitions={'done': 'PLACE',
'failed': 'failed'}
)
smach.StateMachine.add('PLACE',
states.Place(robot, selected_entity_designator, place_position, arm_des),
transitions={'done': 'INSPECT_SOURCE_ENTITY',
'failed': 'failed'}
)
def setup_statemachine(robot):
item = VariableDesignator(resolve_type=Entity)
arm = Designator(robot.leftArm)
sm = StateMachine(outcomes=['done'])
with sm:
# Start challenge via StartChallengeRobust
StateMachine.add('START_CHALLENGE_ROBUST',
StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
transitions={'Done': 'OPEN_DISHWASHER',
'Aborted': 'done',
'Failed': 'OPEN_DISHWASHER'})
StateMachine.add('OPEN_DISHWASHER', NavigateAndOpenDishwasher(robot),
transitions={'succeeded': 'GRAB',
'failed': 'GRAB'})
# StateMachine.add('FIND_OBJECT', FindObject(robot, item),
# transitions={'found': 'GRAB',
# 'not_found': 'SAY_EXIT'})
#
# StateMachine.add('GRAB', Grab(robot, item, arm),
# transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER',
# 'failed': 'FIND_OBJECT2'})
#
# StateMachine.add('FIND_OBJECT2', FindObject(robot, item),
# transitions={'found': 'GRAB2',
# 'not_found': 'SAY_EXIT'})
#
# StateMachine.add('GRAB2', Grab(robot, item, arm),
# transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER',
# 'failed': 'SAY_EXIT'})
StateMachine.add('GRAB', GrabRobust(robot),
transitions={'succeeded': 'PLACE_DISHWASHER',
'failed': 'SAY_EXIT'})
StateMachine.add('PLACE_DISHWASHER', NavigateAndPlaceDishwasher(robot),
transitions={'succeeded': 'SAY_EXIT',
'failed': 'SAY_EXIT'})
StateMachine.add('SAY_EXIT', Say(robot, ["I will move to the exit now. See you guys later!"], block=False),
transitions={'spoken': 'GO_TO_EXIT'})
StateMachine.add('GO_TO_EXIT',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_1), radius=0.7),
transitions={'arrived': 'done',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'})
StateMachine.add('GO_TO_EXIT_2',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_2), radius=0.5),
transitions={'arrived': 'done',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'})
StateMachine.add('GO_TO_EXIT_3',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_3), radius=0.5),
transitions={'arrived': 'done',
'unreachable': 'done',
'goal_not_defined': 'done'})
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_ARM_POSITIONS',
'abort': 'aborted'
})
smach.StateMachine.add('SET_ARM_POSITIONS',
SetPlateCarryingPose(robot),
transitions={'done': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add('WAIT_FOR_KEY_PRESS',
WaitForKeyPress(),
transitions={
'1': 'SAY_IK_KOM',
'2': 'SAY_ALSJEBLIEFT',
'3': 'SAY_ALTIJD',
'4': 'SAY_STEKKER',
'5': 'SAY_OEPS'
})
smach.StateMachine.add('SAY_IK_KOM',
states.Say(robot,
"Ik kom eraan!",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add('SAY_ALSJEBLIEFT',
states.Say(robot,
"Alsjeblieft Rick. De enveloppen.",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add('SAY_ALTIJD',
states.Say(robot,
"Voor jou altijd, Rick.",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add(
'SAY_STEKKER',
states.Say(
robot,
"Ik heb stiekem gekeken, Rick. Maar als ik dat verklap, trekken ze de stekker eruit!",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add(
'RESET_HEAD',
HeadCancel(robot),
transitions={'done': 'GO_BACK_TO_STARTING_POINT'})
smach.StateMachine.add('GO_BACK_TO_STARTING_POINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="starting_point"),
radius=knowledge.back_radius,
speak=False),
transitions={
'arrived': 'done',
'unreachable': 'WAIT_FOR_KEY_PRESS',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add('SAY_OEPS',
states.Say(robot,
"Oeps!",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
with self:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
transitions={"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
radius=0.5),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
radius=0.7),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'ANNOUNCEMENT',
'goal_not_defined': 'ANNOUNCEMENT'})
# Part I: Set a table
smach.StateMachine.add('ANNOUNCEMENT',
states.Say(robot, "Let's see if my master has a task for me! ", block=True),
transitions={'spoken': 'FETCH_COMMAND_I'})
smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0, "set"),
transitions={'done': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot, "What should I serve, master?", block=True),
transitions={'spoken': 'GET_ORDER'})
smach.StateMachine.add('GET_ORDER',
GetBreakfastOrder(robot, knowledge.options,
grasp_designator1,
grasp_designator2,
grasp_designator3,
timeout=15.0),
transitions={'done': 'SET_THE_TABLE'})
smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
ManipulateMachine(robot=robot,
grasp_designator1=grasp_designator1,
grasp_designator2=grasp_designator2,
grasp_designator3=grasp_designator3,
grasp_furniture_id1=knowledge.grasp_furniture_id1,
grasp_furniture_id2=knowledge.grasp_furniture_id2,
place_furniture_id=knowledge.place_furniture_id),
transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION',
'failed': 'RETURN_TO_START_2'})
smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION',
states.Say(robot, "The table is set! Moving to the meeting point for the next task.",
block=False),
transitions={'spoken': 'RETURN_TO_START_2'})
# Part II: Clean the table
smach.StateMachine.add('RETURN_TO_START_2',
states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3),
transitions={'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'})
smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0, "clear"),
transitions={'done': 'CLEAR_UP'})
smach.StateMachine.add('CLEAR_UP', # Clear the table
ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.place_furniture_id,
place_furniture_id1=knowledge.grasp_furniture_id1,
place_furniture_id2=knowledge.grasp_furniture_id2),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am done here"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
with sm:
smach.StateMachine.add("INITIALIZE",
robot_smach_states.Initialize(robot),
transitions={
"initialized": "SAY_WAITING_FOR_TRIGGER",
"abort": "Aborted"
})
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add("START_CHALLENGE_ROBUST",
robot_smach_states.StartChallengeRobust(
robot,
challenge_knowledge.starting_point,
door=False),
transitions={
"Done": "SAY_WAITING_FOR_TRIGGER",
"Failed": "Aborted",
"Aborted": "Aborted"
})
smach.StateMachine.add(
'SAY_WAITING_FOR_TRIGGER',
robot_smach_states.Say(robot, [
"Trigger me if you need me!", "Waiting for trigger",
"Waiting for you to call me!"
],
block=False),
transitions={"spoken": "WAIT_FOR_TRIGGER"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(
robot, ["gpsr"], "/amigo/trigger"),
transitions={
"gpsr": "VERIFY",
"preempted": "VERIFY"
})
smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={
"done": "NAVIGATE_TO_ASK_WAYPOINT",
"failed": "SAY_KNOWLEDGE_NOT_COMPLETE"
})
smach.StateMachine.add(
'SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, [
"My knowledge of the world is not complete!",
"Please give me some more information!"
],
block=False),
transitions={"spoken": "SAY_WAITING_FOR_TRIGGER"})
smach.StateMachine.add("NAVIGATE_TO_ASK_WAYPOINT",
robot_smach_states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.ask_waypoint),
radius=0.3),
transitions={
'arrived':
'DETERMINE_WHAT_TO_CLEAN_INSPECT',
'unreachable':
'DETERMINE_WHAT_TO_CLEAN_INSPECT',
'goal_not_defined':
'DETERMINE_WHAT_TO_CLEAN_INSPECT'
})
smach.StateMachine.add(
"DETERMINE_WHAT_TO_CLEAN_INSPECT",
DetermineWhatToCleanInspect(robot),
transitions={
place["entity_id"]: "CLEAN_INSPECT_%s" % place["entity_id"]
for place in challenge_knowledge.inspection_places
})
for place in challenge_knowledge.inspection_places:
smach.StateMachine.add(
"CLEAN_INSPECT_%s" % place["entity_id"],
CleanInspect(robot, place["entity_id"], place["room_id"],
place["navigate_area"], place["segment_areas"]),
transitions={"done": "NAVIGATE_TO_ASK_WAYPOINT"})
return sm
robot.speech.speak("move") # instruct the user to move the objects
rospy.sleep(1)
robot.speech.speak("Switch")
rospy.sleep(5)
robot.speech.speak(
"clear") #warn the user that a picture will be taken soon
rospy.sleep(1)
if __name__ == '__main__':
rospy.init_node('learn_objects')
if len(sys.argv) < 2:
print("Please specify a robot name")
sys.exit()
robot_name = sys.argv[1]
if robot_name != "hero":
rospy.logwarn(
"Learn_objects is designed to be used with hero. It is unknown how it works for other robots"
)
robot = robot_constructor(robot_name)
location = "hero_case"
location_des = EntityByIdDesignator(robot, location)
learn_objects(robot, location_des)
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot, STARTING_POINT),
transitions={
"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"
})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_1),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
states.AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
states.Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def main():
rospy.init_node("demo")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
rospy.loginfo("[DEMO] Parameters:")
rospy.loginfo("[DEMO] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[DEMO] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[DEMO] running a restart")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
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:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_demo')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger")
while True:
# Navigate to the GPSR meeting point
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
timeout_count = 0
while True:
rospy.loginfo("Waiting for trigger")
trigger.execute()
base_loc = robot.base.get_location()
base_pose = base_loc.frame
print base_pose
location_id = "starting_point"
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
robot.head.look_at_standing_person()
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
timeout_count = 0
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
if timeout_count >= 3:
robot.hmi.restart_dragonfly()
timeout_count = 0
rospy.logwarn("[GPSR] Dragonfly restart")
else:
timeout_count += 1
rospy.logwarn(
"[GPSR] Timeout_count: {}".format(timeout_count))
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) # be sure lights are blue
robot.head.look_at_standing_person()
for arm in robot.arms.itervalues():
arm.reset()
arm.send_gripper_goal('close', 0.0)
robot.torso.reset()
rospy.loginfo("Driving back to the starting point")
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot, id=location_id),
radius=0.3)
nwc.execute()
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_ROBOCUP_ARENA',
'abort': 'aborted'
})
smach.StateMachine.add('STORE_ROBOCUP_ARENA',
StoreRobocupArena(robot),
transitions={'done': 'HEAD_STRAIGHT'})
smach.StateMachine.add('HEAD_STRAIGHT',
HeadStraight(robot),
transitions={'done': 'SAY_INTRO'})
smach.StateMachine.add('SAY_INTRO',
states.Say(
robot,
"Hi, Guide me out of the arena please.",
look_at_standing_person=True),
transitions={'spoken': 'FOLLOW_INITIAL'})
smach.StateMachine.add('FOLLOW_INITIAL',
states.FollowOperator(robot,
operator_timeout=30,
replan=True),
transitions={
'stopped': 'WAIT_FOR_OPERATOR_COMMAND',
'lost_operator':
'WAIT_FOR_OPERATOR_COMMAND',
'no_operator': 'FOLLOW_INITIAL'
})
smach.StateMachine.add('FOLLOW',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=False,
learn_face=False,
replan=True),
transitions={
'stopped': 'WAIT_FOR_OPERATOR_COMMAND',
'lost_operator': 'SAY_GUIDE',
'no_operator': 'SAY_GUIDE'
})
smach.StateMachine.add('WAIT_FOR_OPERATOR_COMMAND',
WaitForOperatorCommand(robot),
transitions={
'follow': 'FOLLOW',
'command': 'SAY_GUIDE'
})
smach.StateMachine.add(
'SAY_GUIDE',
states.Say(robot,
"I will guide you back to the robocup arena!",
look_at_standing_person=True),
transitions={'spoken': 'GUIDE_TO_ROBOCUP_ARENA'})
smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="robocup_arena"),
radius=knowledge.back_radius),
transitions={
'arrived':
'SAY_BACK',
'unreachable':
'GUIDE_TO_ROBOCUP_ARENA_BACKUP',
'goal_not_defined':
'GUIDE_TO_ROBOCUP_ARENA_BACKUP'
})
smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="robocup_arena"),
radius=knowledge.back_radius + 0.1),
transitions={
'arrived': 'SAY_BACK',
'unreachable': 'GUIDE_TO_ROBOCUP_ARENA',
'goal_not_defined': 'GUIDE_TO_ROBOCUP_ARENA'
})
smach.StateMachine.add('SAY_BACK',
states.Say(robot,
"We are back in the robocup arena!",
look_at_standing_person=True),
transitions={'spoken': 'done'})
return sm
def setup_statemachine(robot):
load_waypoints(robot)
operator_id = VariableDesignator(resolve_type=str)
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'STORE_KITCHEN',
'abort': 'aborted'}
)
smach.StateMachine.add('STORE_KITCHEN',
StoreKitchen(robot),
transitions={'done': 'HEAD_STRAIGHT'}
)
smach.StateMachine.add('HEAD_STRAIGHT',
HeadStraight(robot),
transitions={'done': 'SAY_INTRO'}
)
smach.StateMachine.add('SAY_INTRO',
states.Say(robot, "Hi, Show me your restaurant please."),
transitions={'spoken': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('FOLLOW_INITIAL',
states.FollowOperator(robot,
operator_timeout=30,
operator_id_des=operator_id
),
transitions={'stopped': 'STORE',
'lost_operator': 'FOLLOW_INITIAL',
'no_operator': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('FOLLOW',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=False,
operator_id_des=operator_id
),
transitions={'stopped': 'STORE',
'lost_operator': 'FOLLOW_INITIAL',
'no_operator': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('STORE',
StoreWaypoint(robot),
transitions={'done': 'CHECK_KNOWLEDGE',
'continue': 'FOLLOW'}
)
smach.StateMachine.add('CHECK_KNOWLEDGE',
CheckKnowledge(robot),
transitions={'yes': 'SAY_FOLLOW_TO_KITCHEN',
'no':'FOLLOW'}
)
smach.StateMachine.add('SAY_FOLLOW_TO_KITCHEN',
states.Say(robot,
"Please bring me back to the kitchen!"
),
transitions={'spoken': 'FOLLOW_TO_KITCHEN'}
)
smach.StateMachine.add('FOLLOW_TO_KITCHEN',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=False,
operator_id_des=operator_id
),
transitions={'stopped': 'CHECK_IN_KITCHEN',
'lost_operator': 'SAY_GOTO_KITCHEN',
'no_operator': 'SAY_GOTO_KITCHEN'}
)
smach.StateMachine.add('SAY_GOTO_KITCHEN',
states.Say(robot,
"You know what? I will go back to the kitchen on my own!",
block=False
),
transitions={'spoken': 'GOTO_KITCHEN'})
smach.StateMachine.add('GOTO_KITCHEN',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(robot, id="kitchen")
),
transitions={'arrived': 'SAY_IN_KITCHEN',
'unreachable': 'SAY_I_DONT_KNOW_HOW',
'goal_not_defined': 'SAY_I_DONT_KNOW_HOW'}
)
smach.StateMachine.add('SAY_I_DONT_KNOW_HOW',
states.Say(robot,
"Oops, I don't know the way back.",
block=True
),
transitions={'spoken': 'GOTO_KITCHEN'})
# smach.StateMachine.add('FOLLOW_TO_KITCHEN',
# states.FollowOperator(robot,
# operator_timeout=30,
# ask_follow=False
# ),
# transitions={'stopped': 'CHECK_IN_KITCHEN',
# 'lost_operator': 'FOLLOW_TO_KITCHEN_INITIAL',
# 'no_operator': 'FOLLOW_TO_KITCHEN_INITIAL'}
# )
smach.StateMachine.add('CHECK_IN_KITCHEN',
CheckInKitchen(robot),
transitions={'not_in_kitchen': 'FOLLOW_TO_KITCHEN',
'in_kitchen':'SAY_IN_KITCHEN'}
)
smach.StateMachine.add('SAY_IN_KITCHEN',
states.Say(robot,
"We are in the kitchen again!"
),
transitions={'spoken': 'SAY_WHICH_ORDER'}
)
# Where to take the order from?
smach.StateMachine.add('SAY_WHICH_ORDER', states.Say(robot, "From which table should I take the first order?"), transitions={ 'spoken' :'HEAR_WHICH_ORDER'})
smach.StateMachine.add('HEAR_WHICH_ORDER', HearWhichTable(robot),
transitions={ 'no_result' :'SAY_WHICH_ORDER', 'one' : 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_1', 'two': 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_2', 'three' : "FIRST_SAY_TAKE_ORDER_FROM_TABLE_3"})
# ############## first table ##############
for i, name in tables.iteritems():
next_i = i+1
if next_i > 3:
next_i = 1
smach.StateMachine.add('FIRST_SAY_TAKE_ORDER_FROM_TABLE_%d'%i, states.Say(robot, "Okay, I will take an order from table %d"%i, block=False),
transitions={ 'spoken' :'FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i})
smach.StateMachine.add('FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i, states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=name), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'FIRST_ASK_ORDER_TABLE_%d'%i, 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i})
smach.StateMachine.add('FIRST_ASK_ORDER_TABLE_%d'%i, AskOrder(robot, name),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'orders_done' : 'SAY_ORDERS_DONE'})
# ############## Loop over the reset of the tables until we have a beverage and a combo ##############
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_1', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="one"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_1', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_2'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_1', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_1'})
smach.StateMachine.add('HEAD_DOWN_TABLE_1', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_1'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_1', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_2', 'yes':'ASK_ORDER_TABLE_1','no':'NAVIGATE_TO_WAYPOINT_TABLE_2'})
smach.StateMachine.add('ASK_ORDER_TABLE_1', AskOrder(robot, "one"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="two"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_2', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_3'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_2', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_2'})
smach.StateMachine.add('HEAD_DOWN_TABLE_2', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_2'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_2', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_3', 'yes':'ASK_ORDER_TABLE_2','no':'NAVIGATE_TO_WAYPOINT_TABLE_3'})
smach.StateMachine.add('ASK_ORDER_TABLE_2', AskOrder(robot, "two"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_3', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="three"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_3', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_1'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_3', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_3'})
smach.StateMachine.add('HEAD_DOWN_TABLE_3', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_3'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_3', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_1', 'yes':'ASK_ORDER_TABLE_3','no':'NAVIGATE_TO_WAYPOINT_TABLE_1'})
smach.StateMachine.add('ASK_ORDER_TABLE_3', AskOrder(robot, "three"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('SAY_ORDERS_DONE', states.Say(robot, "I received enough orders for now, going back to the kitchen!", block=False),
transitions={ 'spoken' :'NAVIGATE_BACK_TO_THE_KITCHEN'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.2),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.4),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'SPEAK_ORDERS', 'goal_not_defined':'SPEAK_ORDERS'})
smach.StateMachine.add('SPEAK_ORDERS', SpeakOrders(robot),
transitions={ 'spoken' :'STORE_BEVERAGE_SIDE'})
smach.StateMachine.add('STORE_BEVERAGE_SIDE', StoreBeverageSide(robot),
transitions={ 'done' : 'NAVIGATE_TO_BEVERAGES'})
smach.StateMachine.add('NAVIGATE_TO_BEVERAGES', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'NAVIGATE_TO_BEVERAGES_BACKUP', 'goal_not_defined':'NAVIGATE_TO_BEVERAGES_BACKUP'})
smach.StateMachine.add('NAVIGATE_TO_BEVERAGES_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS+0.1),
transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'STORE_BEVERAGE_SIDE', 'goal_not_defined':'STORE_BEVERAGE_SIDE'})
smach.StateMachine.add('SPEAK_I_SEE_THE_BEVERAGES', states.Say(robot, "The beverages are in front of me", block=False),
transitions={ 'spoken' :'DELIVER_BEVERAGE'})
smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"),
transitions={'succeeded':'NAVIGATE_TO_KITCHEN', 'failed':'NAVIGATE_TO_KITCHEN'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'NAVIGATE_TO_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_TO_KITCHEN_BACKUP'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.1),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'DELIVER_COMBO', 'goal_not_defined':'DELIVER_COMBO'})
smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"),
transitions={'succeeded':'NAVIGATE_BACK_TO_THE_KITCHEN_2', 'failed':'NAVIGATE_BACK_TO_THE_KITCHEN_2'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_DONE_WITH_CHALLENGE', 'unreachable':'SAY_DONE_WITH_CHALLENGE', 'goal_not_defined':'SAY_DONE_WITH_CHALLENGE'})
smach.StateMachine.add('SAY_DONE_WITH_CHALLENGE', states.Say(robot, "I'm done with this challenge and you are the banana king!"), transitions={ 'spoken' :'done'})
analyse_designators(sm, "restaurant")
return sm
def setup_statemachine(robot):
furniture = EntityByIdDesignator(robot, 'selected_furniture')
# arm_designator = ArmDesignator(robot.arms, robot.arms['left'])
arm_designator = UnoccupiedArmDesignator(robot.arms, robot.arms['left'])
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add("START_CHALLENGE_ROBUST",
StartChallengeRobust(robot, challenge_knowledge.initial_pose),
transitions={"Done": "NAVIGATE_TO_SSL_WAYPOINT",
"Failed": "Aborted",
"Aborted": "Aborted"})
# smach.StateMachine.add('SAY_STARTING_OPEN_CHALLENGE',
# robot_smach_states.Say(robot, ["Hi there, welcome to the open challenge!"], block=False),
# transitions={"spoken": "NAVIGATE_TO_SSL_WAYPOINT"})
smach.StateMachine.add("NAVIGATE_TO_SSL_WAYPOINT",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.ssl_waypoint),
radius=0.3),
transitions={'arrived': 'SSL_DEMO',
'unreachable': 'SSL_DEMO',
'goal_not_defined': 'SSL_DEMO'})
smach.StateMachine.add("SSL_DEMO",
SSLDemo(robot),
transitions={"done": "NAVIGATE_TO_LASER_DEMO", 'preempted': 'Aborted'})
smach.StateMachine.add("NAVIGATE_TO_LASER_DEMO",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.raytrace_waypoint),
radius=0.3),
transitions={'arrived': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO',
'unreachable': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO',
'goal_not_defined': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO'})
smach.StateMachine.add("RESET_HEAD_BEFORE_RAYTRACE_DEMO",
ResetHead(robot),
transitions={'done': 'SAY_PEOPLE_DETECTOR'})
smach.StateMachine.add("SAY_PEOPLE_DETECTOR",
Say(robot, "Now I will show you my awesome people detector"),
transitions={"spoken": "WAIT_FOR_TRIGGER_BEFORE_RAYTRACE_DEMO"})
smach.StateMachine.add("WAIT_FOR_TRIGGER_BEFORE_RAYTRACE_DEMO",
WaitForTrigger(robot, ["continue"], "/amigo/trigger"),
transitions={'continue': 'RAYTRACE_DEMO',
'preempted': 'RAYTRACE_DEMO'})
smach.StateMachine.add("RAYTRACE_DEMO",
RayTraceDemo(robot, breakout_id=challenge_knowledge.raytrace_waypoint),
transitions={"done": "SAY_RAYTRACE_SELECTOR"})
smach.StateMachine.add("SAY_RAYTRACE_SELECTOR",
Say(robot, "You can interact with me by pointing at objects!"),
transitions={"spoken": "RAYTRACE_SELECTOR"})
smach.StateMachine.add("RAYTRACE_SELECTOR",
SimpleRayTraceSelector(robot, waypoint=None, furniture_designator=furniture),
transitions={
"waypoint": 'NAVIGATE_TO_WAYPOINT',
"furniture": 'NAVIGATE_TO_FURNITURE',
"grasp": 'INSPECT_AND_GRAB',
"done": 'RAYTRACE_SELECTOR'
})
smach.StateMachine.add("NAVIGATE_TO_FURNITURE",
NavigateToSymbolic(robot,
entity_designator_area_name_map={furniture: 'in_front_of'},
entity_lookat_designator=furniture),
transitions={
'arrived': 'NAVIGATE_BACK_TO_LASER_DEMO',
'unreachable': 'NAVIGATE_BACK_TO_LASER_DEMO',
'goal_not_defined': 'NAVIGATE_BACK_TO_LASER_DEMO',
})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id='final_waypoint'),
radius=0.3),
transitions={'arrived': 'NAVIGATE_BACK_TO_LASER_DEMO',
'unreachable': 'NAVIGATE_BACK_TO_LASER_DEMO',
'goal_not_defined': 'NAVIGATE_BACK_TO_LASER_DEMO'})
smach.StateMachine.add("INSPECT_AND_GRAB",
InspectAndGrab(robot, supporting_entity_designator=furniture,
arm_designator=arm_designator),
transitions={
'succeeded': 'NAVIGATE_BACK_TO_LASER_DEMO',
'inspect_failed': 'NAVIGATE_BACK_TO_LASER_DEMO',
'grasp_failed': 'NAVIGATE_BACK_TO_LASER_DEMO'
})
smach.StateMachine.add("HANDOVER_TO_HUMAN", HandoverToHuman(robot, arm_designator, timeout=10),
transitions={
'succeeded': 'NAVIGATE_BACK_TO_LASER_DEMO',
'failed': 'NAVIGATE_BACK_TO_LASER_DEMO',
})
smach.StateMachine.add("NAVIGATE_BACK_TO_LASER_DEMO",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.raytrace_waypoint),
radius=0.3),
transitions={'arrived': 'RAYTRACE_SELECTOR',
'unreachable': 'RAYTRACE_SELECTOR',
'goal_not_defined': 'RAYTRACE_SELECTOR'})
# smach.StateMachine.add("NAVIGATE_TO_ORDER_COUNTER",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(
# robot=robot,
# id=challenge_knowledge.order_counter_waypoint),
# radius=0.3),
# transitions={'arrived': 'ORDER_COUNTER',
# 'unreachable': 'ORDER_COUNTER',
# 'goal_not_defined': 'ORDER_COUNTER'})
#
# smach.StateMachine.add("ORDER_COUNTER",
# OrderCounter(robot, room_id=challenge_knowledge.audience_room,
# beercounter=beercounter),
# transitions={"done": "NAVIGATE_TO_HSR_DEMO"})
#
# smach.StateMachine.add("NAVIGATE_TO_HSR_DEMO",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(
# robot=robot,
# id=challenge_knowledge.hsr_demo_waypoint),
# radius=0.025),
# transitions={'arrived': 'WAIT_FOR_BEER',
# 'unreachable': 'CANNOT_REACH_BEER_LOCATION',
# 'goal_not_defined': 'CANNOT_REACH_BEER_LOCATION'})
#
# smach.StateMachine.add("CANNOT_REACH_BEER_LOCATION",
# robot_smach_states.Say(robot, "I cannot reach my beer buddy, let's give it another try"),
# transitions={"spoken": "Done"})
#
# smach.StateMachine.add("WAIT_FOR_BEER",
# HsrInteraction(robot=robot, beercounter=beercounter),
# transitions={"done": "RETURN_TO_AUDIENCE"})
#
# smach.StateMachine.add("RETURN_TO_AUDIENCE",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(
# robot=robot,
# id=challenge_knowledge.order_counter_waypoint),
# radius=0.3),
# transitions={'arrived': 'SAY_BEER',
# 'unreachable': 'SAY_BEER',
# 'goal_not_defined': 'SAY_BEER'})
#
# smach.StateMachine.add("SAY_BEER",
# robot_smach_states.Say(robot, "Hey guys, here's your beer. That will be all. "
# "By the way, if you leave the balcony door open,"
# "birds will fly in"),
# transitions={"spoken": "Done"})
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
challenge_knowledge.starting_point,
use_entry_points=True),
transitions={
"Done": "SAY_GOTO_TARGET2",
"Aborted": "SAY_GOTO_TARGET2",
"Failed": "SAY_GOTO_TARGET2"
})
smach.StateMachine.add(
'SAY_GOTO_TARGET1',
states.Say(robot, [
"I will go to target 1 now", "I will now go to target 1",
"Lets go to target 1", "Going to target 1"
],
block=False),
transitions={'spoken': 'GOTO_TARGET1'})
######################################################################################################################################################
#
# TARGET 1
#
######################################################################################################################################################
smach.StateMachine.add('GOTO_TARGET1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target1),
challenge_knowledge.target1_radius1),
transitions={
'arrived': 'SAY_TARGET1_REACHED',
'unreachable': 'RESET_ED_TARGET1',
'goal_not_defined': 'RESET_ED_TARGET1'
})
smach.StateMachine.add(
'SAY_TARGET1_REACHED',
states.Say(robot, [
"Reached target 1", "I have arrived at target 1",
"I am now at target 1"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET3'})
smach.StateMachine.add('RESET_ED_TARGET1',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET1_BACKUP'})
smach.StateMachine.add('GOTO_TARGET1_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target1),
challenge_knowledge.target1_radius2),
transitions={
'arrived': 'SAY_TARGET1_REACHED',
'unreachable': 'TIMEOUT1',
'goal_not_defined': 'TIMEOUT1'
})
smach.StateMachine.add('TIMEOUT1',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET1',
'time_out': 'SAY_TARGET1_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_TARGET1_FAILED',
states.Say(robot, [
"I am not able to reach target 1", "I cannot reach target 1",
"Target 1 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET3'})
######################################################################################################################################################
#
# TARGET 2
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_TARGET2',
states.Say(robot, [
"I will go to target 2 now", "I will now go to target 2",
"Lets go to target 2", "Going to target 2"
],
block=False),
transitions={'spoken': 'GOTO_TARGET2_PRE'})
smach.StateMachine.add(
'GOTO_TARGET2_PRE',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.target2_pre),
challenge_knowledge.target2_pre_radius1,
EntityByIdDesignator(robot, id=challenge_knowledge.target2)),
transitions={
'arrived': 'GOTO_TARGET2',
'unreachable': 'TIMEOUT2',
'goal_not_defined': 'TIMEOUT2'
})
smach.StateMachine.add('GOTO_TARGET2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius1),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'DETERMINE_OBJECT',
'goal_not_defined': 'DETERMINE_OBJECT'
})
smach.StateMachine.add(
'DETERMINE_OBJECT',
DetermineObject(robot, challenge_knowledge.target2,
challenge_knowledge.target2_obstacle_radius),
transitions={
'done': 'GOTO_TARGET2_AGAIN',
'timeout': 'GOTO_TARGET2_AGAIN'
})
smach.StateMachine.add('GOTO_TARGET2_AGAIN',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius1),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'RESET_ED_TARGET2',
'goal_not_defined': 'RESET_ED_TARGET2'
})
smach.StateMachine.add(
'SAY_TARGET2_REACHED',
states.Say(robot, [
"Reached target 2", "I have arrived at target 2",
"I am now at target 2"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET1'})
smach.StateMachine.add('RESET_ED_TARGET2',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET2_BACKUP'})
smach.StateMachine.add('GOTO_TARGET2_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius2),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'TIMEOUT2',
'goal_not_defined': 'TIMEOUT2'
})
smach.StateMachine.add('TIMEOUT2',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET2_PRE',
'time_out': 'SAY_TARGET2_FAILED'
})
smach.StateMachine.add(
'SAY_TARGET2_FAILED',
states.Say(robot, [
"I am unable to reach target 2", "I cannot reach target 2",
"Target 2 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET1'})
######################################################################################################################################################
#
# TARGET 3
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_TARGET3',
states.Say(robot, [
"I will go to target 3 now", "I will now go to target 3",
"Lets go to target 3", "Going to target 3"
],
block=False),
transitions={'spoken': 'GOTO_TARGET3'})
smach.StateMachine.add('GOTO_TARGET3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target3),
challenge_knowledge.target3_radius1),
transitions={
'arrived': 'SAY_TARGET3_REACHED',
'unreachable': 'RESET_ED_TARGET3',
'goal_not_defined': 'RESET_ED_TARGET3'
})
smach.StateMachine.add(
'SAY_TARGET3_REACHED',
states.Say(robot, [
"Reached target 3", "I have arrived at target 3",
"I am now at target 3"
],
block=True),
transitions={'spoken': 'TURN'})
smach.StateMachine.add('RESET_ED_TARGET3',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET3_BACKUP'})
smach.StateMachine.add('GOTO_TARGET3_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target3),
challenge_knowledge.target3_radius2),
transitions={
'arrived': 'SAY_TARGET3_REACHED',
'unreachable': 'TIMEOUT3',
'goal_not_defined': 'TIMEOUT3'
})
smach.StateMachine.add('TIMEOUT3',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET3',
'time_out': 'SAY_TARGET3_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_TARGET3_FAILED',
states.Say(robot, [
"I am unable to reach target 3", "I cannot reach target 3",
"Target 3 is unreachable"
],
block=True),
transitions={'spoken': 'TURN'})
######################################################################################################################################################
#
# Follow waiter
#
######################################################################################################################################################
smach.StateMachine.add('TURN',
Turn(robot, challenge_knowledge.rotation),
transitions={'turned': 'SAY_STAND_IN_FRONT'})
smach.StateMachine.add(
'SAY_STAND_IN_FRONT',
states.Say(robot,
"Please stand in front of me!",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'FOLLOW_WITH_DOOR_CHECK'})
# TODO: Fix concurrence
door_id_designator = VariableDesignator(
challenge_knowledge.target_door_1)
open_door_wp1_des = VariableDesignator(resolve_type=str)
open_door_wp2_des = VariableDesignator(resolve_type=str)
cc = smach.Concurrence(
['stopped', 'no_operator', 'lost_operator'],
default_outcome='no_operator',
child_termination_cb=lambda so: True,
outcome_map={
'stopped': {
'FOLLOW_OPERATOR': 'stopped'
},
# 'stopped': {'FOLLOW_OPERATOR': 'stopped', 'DETERMINE_DOOR': 'door_found'},
'no_operator': {
'FOLLOW_OPERATOR': 'no_operator'
},
# 'no_operator': {'FOLLOW_OPERATOR': 'no_operator', 'DETERMINE_DOOR': 'door_found'},
# 'lost_operator': {'FOLLOW_OPERATOR': 'lost_operator', 'DETERMINE_DOOR': 'preempted'},
'lost_operator': {
'FOLLOW_OPERATOR': 'lost_operator'
}
})
with cc:
smach.Concurrence.add('FOLLOW_OPERATOR',
states.FollowOperator(robot, replan=True))
smach.Concurrence.add('DETERMINE_DOOR',
DetermineDoor(robot, door_id_designator))
smach.StateMachine.add('FOLLOW_WITH_DOOR_CHECK',
cc,
transitions={
'no_operator': 'FOLLOW_WITH_DOOR_CHECK',
'stopped': 'SAY_SHOULD_I_RETURN',
'lost_operator': 'SAY_SHOULD_I_RETURN'
})
# smach.StateMachine.add( 'FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True), transitions={ 'no_operator':'SAY_SHOULD_I_RETURN', 'stopped' : 'SAY_SHOULD_I_RETURN', 'lost_operator' : 'SAY_SHOULD_I_RETURN'})
smach.StateMachine.add('SAY_SHOULD_I_RETURN',
states.Say(robot,
"Should I return to target 3?",
look_at_standing_person=True),
transitions={'spoken': 'HEAR_SHOULD_I_RETURN'})
smach.StateMachine.add('HEAR_SHOULD_I_RETURN',
states.HearOptions(robot, ["yes", "no"]),
transitions={
'no_result': 'SAY_STAND_IN_FRONT',
"yes": "SELECT_WAYPOINTS",
"no": "SAY_STAND_IN_FRONT"
})
smach.StateMachine.add('SELECT_WAYPOINTS',
SelectWaypoints(door_id_designator,
open_door_wp1_des,
open_door_wp2_des),
transitions={'done': 'SAY_GOBACK_ARENA'})
######################################################################################################################################################
#
# RETURN TO ARENA DOOR
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOBACK_ARENA',
states.Say(robot, [
"I will go back to the arena", "I will return to the arena",
"Lets return to the arena", "Going back to the arena",
"Returning to the arena"
],
block=False),
transitions={'spoken': 'GOTO_ARENA_DOOR'})
smach.StateMachine.add(
'GOTO_ARENA_DOOR',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot, id_designator=door_id_designator),
challenge_knowledge.target_door_radius),
transitions={
'arrived': 'ARENA_DOOR_REACHED',
'unreachable': 'RESET_ED_ARENA_DOOR',
'goal_not_defined': 'RESET_ED_ARENA_DOOR'
})
smach.StateMachine.add('ARENA_DOOR_REACHED',
states.Say(robot, [
"I am at the door of the arena",
"I have arrived at the door of the arena",
"I am now at the door of the arena"
],
block=True),
transitions={'spoken': 'SAY_OPEN_DOOR'})
smach.StateMachine.add('RESET_ED_ARENA_DOOR',
states.ResetED(robot),
transitions={'done': 'GOTO_ARENA_DOOR_BACKUP'})
smach.StateMachine.add(
'GOTO_ARENA_DOOR_BACKUP',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot, id_designator=door_id_designator),
challenge_knowledge.target_door_radius),
transitions={
'arrived': 'ARENA_DOOR_REACHED',
'unreachable': 'TIMEOUT_ARENA_DOOR',
'goal_not_defined': 'TIMEOUT_ARENA_DOOR'
})
smach.StateMachine.add('TIMEOUT_ARENA_DOOR',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds_door),
transitions={
'not_yet': 'GOTO_ARENA_DOOR',
'time_out': 'SAY_GOTO_ARENA_DOOR_FAILED'
})
smach.StateMachine.add('SAY_GOTO_ARENA_DOOR_FAILED',
states.Say(robot, [
"I am unable to reach the arena door",
"I cannot reach the arena door",
"The arena door is unreachable"
],
block=True),
transitions={'spoken': 'Done'})
######################################################################################################################################################
#
# Opening Door
#
######################################################################################################################################################
smach.StateMachine.add(
'OPEN_DOOR',
states.OpenDoorByPushing(
robot,
EdEntityDesignator(robot, id_designator=open_door_wp1_des),
EdEntityDesignator(robot, id_designator=open_door_wp2_des)),
transitions={
'succeeded': 'SAY_RETURN_TARGET3',
'failed': 'TIMEOUT_ARENA_DOOR_OPENING'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR',
states.Say(robot, [
"I am going to open the door",
"Going to open the door of the arena", "Door, open sesame"
],
block=True),
transitions={'spoken': 'OPEN_DOOR'})
smach.StateMachine.add(
'SAY_OPEN_DOOR_AGAIN',
states.Say(robot, [
"I failed to open the door. I will try it again",
"Let me try again to open the door"
],
block=True),
transitions={'spoken': 'OPEN_DOOR'})
smach.StateMachine.add('TIMEOUT_ARENA_DOOR_OPENING',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'SAY_OPEN_DOOR_AGAIN',
'time_out': 'SAY_OPEN_DOOR_FAILED'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR_FAILED',
states.Say(robot, [
"I was not able to open the door. I am done with this challange",
"I was not able to open the door. I am done with this challange"
],
block=True),
transitions={'spoken': 'Done'})
######################################################################################################################################################
#
# RETURN TO TARGET 3
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_RETURN_TARGET3',
states.Say(robot, [
"I will go back to target 3 now", "I will return to target 3",
"Lets go to target 3 again", "Going to target 3, again"
],
block=False),
transitions={'spoken': 'RETURN_TARGET3'})
smach.StateMachine.add(
'RETURN_TARGET3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot, id=challenge_knowledge.target4),
challenge_knowledge.target4_radius1),
transitions={
'arrived': 'SAY_TARGET3_RETURN_REACHED',
'unreachable': 'RESET_ED_RETURN_TARGET3',
'goal_not_defined': 'RESET_ED_RETURN_TARGET3'
})
smach.StateMachine.add(
'SAY_TARGET3_RETURN_REACHED',
states.Say(robot, [
"Reached target 3 again", "I have arrived at target 3 again",
"I am now at target 3 again"
],
block=True),
transitions={'spoken': 'SAY_GOTO_EXIT'})
smach.StateMachine.add(
'RESET_ED_RETURN_TARGET3',
states.ResetED(robot),
transitions={'done': 'GOTO_RETURN_TARGET3_BACKUP'})
smach.StateMachine.add('GOTO_RETURN_TARGET3_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target4),
challenge_knowledge.target4_radius2),
transitions={
'arrived': 'SAY_TARGET3_RETURN_REACHED',
'unreachable': 'TIMEOUT3_RETURN',
'goal_not_defined': 'TIMEOUT3_RETURN'
})
smach.StateMachine.add('TIMEOUT3_RETURN',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'RETURN_TARGET3',
'time_out': 'SAY_RETURN_TARGET3_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_RETURN_TARGET3_FAILED',
states.Say(robot, [
"I am unable to reach target 3 again",
"I cannot reach target 3 again", "Target 3 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_EXIT'})
######################################################################################################################################################
#
# TARGET EXIT
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_EXIT',
states.Say(robot, [
"I will move to the exit now. See you guys later!",
"I am done with this challenge. Going to the exit"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit1),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_EXIT',
'goal_not_defined': 'RESET_ED_EXIT'
})
smach.StateMachine.add('RESET_ED_EXIT',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit2),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_EXIT2',
'goal_not_defined': 'RESET_ED_EXIT2'
})
smach.StateMachine.add('RESET_ED_EXIT2',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP2'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit3),
radius=0.6),
transitions={
'arrived': 'GO_TO_EXIT_BACKUP3',
'unreachable': 'RESET_ED_EXIT3',
'goal_not_defined': 'RESET_ED_EXIT3'
})
smach.StateMachine.add('RESET_ED_EXIT3',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP3'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit4),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'AT_END',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "navigation")
return sm
def __init__(self, robot, location_id, room_id, navigate_area,
segment_areas, known_types):
smach.StateMachine.__init__(self, outcomes=['done'])
# Set up the designators for this machine
e_classifications_des = VariableDesignator(
[],
resolve_type=[ClassificationResult],
name='e_classifications_des')
e_des = EdEntityDesignator(robot, id=location_id, name='e_des')
room_des = EntityByIdDesignator(robot, id=room_id, name='room_des')
with self:
# Loop over the areas that we have to segment and handle segmented objects for the specified navigation area
for i, segment_area in enumerate(segment_areas):
smach.StateMachine.add("RESET_ED_%d" % i,
robot_smach_states.ResetED(robot),
transitions={'done': 'NAVIGATE_%d' % i})
smach.StateMachine.add("NAVIGATE_%d" % i,
robot_smach_states.NavigateToSymbolic(
robot, {
e_des: navigate_area,
room_des: "in"
}, e_des),
transitions={
'arrived':
'SEGMENT_SAY_%d' % i,
'unreachable':
"SAY_UNREACHABLE_%d" % i,
'goal_not_defined':
"SAY_UNREACHABLE_%d" % i
})
smach.StateMachine.add(
"SEGMENT_SAY_%d" % i,
robot_smach_states.Say(
robot,
["Looking %s the %s" % (segment_area, location_id)],
block=False),
transitions={"spoken": "SEGMENT_%d" % i})
smach.StateMachine.add(
'SEGMENT_%d' % i,
robot_smach_states.SegmentObjects(
robot, e_classifications_des.writeable, e_des,
segment_area),
transitions={'done': "HANDLE_DETECTED_ENTITIES_%d" % i})
# Determine the next state, either it is the next iter or done
next_state = "RESET_ED_%d" % (
i + 1) if i + 1 < len(segment_areas) else "done"
smach.StateMachine.add(
"SAY_UNREACHABLE_%d" % i,
robot_smach_states.Say(
robot, ["I failed to inspect the %s" % location_id],
block=True),
transitions={"spoken": next_state})
smach.StateMachine.add("HANDLE_DETECTED_ENTITIES_%d" % i,
HandleDetectedEntities(
robot, e_classifications_des,
known_types, location_id,
segment_area),
transitions={"done": next_state})
def main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
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:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_gpsr')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
while True:
# Navigate to the GPSR meeting point
if not skip and not finished:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
while True:
while True and not test:
try:
robot.hmi.query(description="",
grammar="T -> %s" % robot_name,
target="T")
except hmi.TimeoutException:
continue
else:
break
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
continue
if not test:
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no',
'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) #be sure lights are blue
robot.head.look_at_standing_person()
robot.leftArm.reset()
robot.leftArm.send_gripper_goal('close', 0.0)
robot.rightArm.reset()
robot.rightArm.send_gripper_goal('close', 0.0)
robot.torso.reset()
if task_result.succeeded:
# Keep track of the number of performed tasks
no_of_tasks_performed += 1
if no_of_tasks_performed >= no_of_tasks:
finished = True
# If we succeeded, we can say something optimistic after reporting to the operator
if no_of_tasks_performed == 1:
task_word = "task"
else:
task_word = "tasks"
report += " I performed {} {} so far, still going strong!".format(
no_of_tasks_performed, task_word)
if rospy.get_time() - start_time > 60 * 15:
finished = True
if finished and not skip:
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.exit_waypoint),
radius=0.3)
nwc.execute()
robot.speech.speak("Thank you very much, and goodbye!", block=True)
break
def main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
eegpsr = rospy.get_param('~eegpsr', False)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
if time_limit == 0:
time_limit = 999
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
rospy.loginfo("[GPSR] time_limit = {}".format(time_limit))
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
robot = get_robot(robot_name)
if eegpsr:
knowledge = load_knowledge('challenge_eegpsr')
else:
knowledge = load_knowledge('challenge_gpsr')
conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar,
knowledge.grammar_target,
knowledge)
conversation_engine.test = test
conversation_engine.skip = skip
conversation_engine.tasks_to_be_done = no_of_tasks
conversation_engine.time_limit = time_limit
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
conversation_engine._start_wait_for_command(knowledge.grammar,
knowledge.grammar_target)
rospy.spin()
