def __init__(self, robot):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)
with self:
StateMachine.add("NAVIGATE_BACK_TO_DISHWASHER",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'CUSTOM_PLACE',
'unreachable': 'SAY_DISHWASHER_NOT_REACHABLE',
'goal_not_defined': 'SAY_DISHWASHER_NOT_REACHABLE'})
StateMachine.add('SAY_DISHWASHER_NOT_REACHABLE', Say(robot, [
"I cannot reach the dishwasher, let me try again in a couple of seconds! Hopefully this will go right this time."],
block=True),
transitions={'spoken': 'NAVIGATE_BACK_TO_DISHWASHER2'})
StateMachine.add("NAVIGATE_BACK_TO_DISHWASHER2",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'CUSTOM_PLACE',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add('CUSTOM_PLACE', CustomPlace(robot),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
source_entity_designator = EdEntityDesignator(robot, id=source_entity)
navigation_area_designator1 = VariableDesignator('in_front_of')
navigation_area_designator2 = VariableDesignator('in_back_of')
with self:
StateMachine.add("NAVIGATE_TO_INSPECT",
NavigateToSymbolic(robot, {source_entity_designator: navigation_area_designator1},
source_entity_designator),
transitions={'arrived': 'SIMPLE_GRASP',
'unreachable': 'NAVIGATE_TO_INSPECT2',
'goal_not_defined': 'NAVIGATE_TO_INSPECT2'})
StateMachine.add('SIMPLE_GRASP', FindAndGrab(robot),
transitions={'succeeded': 'succeeded',
'failed': 'NAVIGATE_TO_INSPECT2'})
StateMachine.add("NAVIGATE_TO_INSPECT2",
NavigateToSymbolic(robot, {source_entity_designator: navigation_area_designator2},
source_entity_designator),
transitions={'arrived': 'SIMPLE_GRASP2',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add('SIMPLE_GRASP2', FindAndGrab(robot),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def base_move(pre):
global base_moving_base
global base_plan_viable
global wp
global objects_to_navigate_ds
global base_goal_reached
base_goal_reached = False
wp = objects_to_navigate_ds.pop()
navSM = NavigateToSymbolic(robot, {wp: "in_front_of"}, wp)
# Create a simple SMACH state machine
sm = smach.StateMachine(outcomes=['moving','unreachable','goal_not_defined', 'preempted'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('GET_PLAN', getPlan(robot, navSM.generateConstraint, True),
transitions={'unreachable' : 'unreachable',
'goal_not_defined' : 'goal_not_defined',
'goal_ok' : 'moving',
'preempted' : 'preempted'})
# Execute SMACH plan
outcome = sm.execute()
if outcome == 'moving':
base_moving_base = True
elif outcome == 'unreachable'or 'goal_not_defined':
base_plan_viable = 2
objects_to_navigate_ds.append(wp)
Amigo_AssignInputVariables()
def __init__(self,
robot,
table_id,
table_navigation_area,
table_close_navigation_area,
placement_height=0.7):
StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
input_keys=["item_picked"])
table = EdEntityDesignator(robot=robot, id=table_id)
with self:
# StateMachine.add("NAVIGATE_TO_TABLE",
# NavigateToSymbolic(robot, {table: table_navigation_area}, table),
# transitions={'arrived': 'NAVIGATE_TO_TABLE_CLOSE',
# 'unreachable': 'failed',
# 'goal_not_defined': 'failed'})
StateMachine.add(
"NAVIGATE_TO_TABLE_CLOSE",
NavigateToSymbolic(robot, {table: table_close_navigation_area},
table),
transitions={
'arrived': 'PLACE_ITEM_ON_TABLE',
'unreachable': 'SAY_PICK_AWAY_THE_CHAIR',
'goal_not_defined': 'failed'
})
StateMachine.add("FORCE_DRIVE",
ForceDrive(robot, -0.1, 0, 0, 5.0),
transitions={'done': 'SAY_PICK_AWAY_THE_CHAIR'})
StateMachine.add(
"SAY_PICK_AWAY_THE_CHAIR",
Say(
robot,
"Please pick away the chair, I cannot get close enough to the {}"
.format(table_id)),
transitions={'spoken': 'WAIT_FOR_PICK_AWAY_CHAIR'})
StateMachine.add('WAIT_FOR_PICK_AWAY_CHAIR',
WaitTime(robot, 5),
transitions={
'waited': 'SAY_THANKS',
'preempted': 'failed'
})
StateMachine.add('SAY_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_TO_TABLE_CLOSE'})
StateMachine.add("PLACE_ITEM_ON_TABLE",
PlaceItemOnTable(robot, table_id,
placement_height),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot, cupboard_id, cupboard_navigation_area, required_items):
StateMachine.__init__(self, outcomes=["succeeded", "failed"], output_keys=["item_picked"])
cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
with self:
StateMachine.add("NAVIGATE_TO_CUPBOARD",
NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard),
transitions={'arrived': 'PICK_ITEM_FROM_CUPBOARD',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("PICK_ITEM_FROM_CUPBOARD", PickItemFromCupboardDrawer(robot, cupboard_id, required_items),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot, dishwasher_id, dishwasher_navigate_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)
with self:
StateMachine.add("NAVIGATE_TO_DISHWASHER",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'OPEN_DISHWASHER',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("OPEN_DISHWASHER",
OpenDishwasher(robot, dishwasher_id),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot, rack_id, rack_navigation_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
rack = EdEntityDesignator(robot=robot, id=rack_id)
with self:
StateMachine.add("NAVIGATE_TO_RACK",
NavigateToSymbolic(robot,
{rack: rack_navigation_area},
rack),
transitions={
'arrived': 'GRAB_RACK',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
StateMachine.add("GRAB_RACK",
GrabRack(robot, rack_id),
transitions={'done': 'succeeded'})
def __init__(self, robot, cupboard_id, cupboard_navigation_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
with self:
StateMachine.add(
"NAVIGATE_TO_CUPBOARD",
NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area},
cupboard),
transitions={
'arrived': 'CLOSE_CUPBOARD',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
StateMachine.add("CLOSE_CUPBOARD",
CloseCupboard(robot, cupboard_id),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
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