def __init__(self, robot, item):
StateMachine.__init__(self, outcomes=['found', 'not_found'])
source_entity_designator = EdEntityDesignator(robot, id=source_entity)
# description_designator = VariableDesignator({
# 'type': 'cup'
# })
area_name_designator = VariableDesignator('on_top_of')
navigation_area_designator1 = VariableDesignator('in_back_of')
navigation_area_designator2 = VariableDesignator('in_front_of')
with self:
StateMachine.add('FIND', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator1, item),
transitions={'found': 'found',
'not_found': 'FIND2'})
StateMachine.add('FIND2', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator2, item),
transitions={'found': 'found',
'not_found': 'FIND3'})
StateMachine.add('FIND3', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator1, item),
transitions={'found': 'found',
'not_found': 'FIND4'})
StateMachine.add('FIND4', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator2, item),
transitions={'found': 'found',
'not_found': 'not_found'})
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 __init__(self, robot, knowledge, source_entity_designator, description_designator, area_name_designator,
navigation_area_designator, found_entity_designator):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'inspect_failed', 'not_found'])
segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult])
with self:
smach.StateMachine.add('INSPECT_SOURCE_ENTITY',
states.world_model.Inspect(robot=robot,
entityDes=source_entity_designator,
objectIDsDes=segmented_entities_designator,
searchArea=area_name_designator,
navigation_area=navigation_area_designator),
transitions={'done': 'CHECK_IF_ENTITY_FOUND',
'failed': 'inspect_failed'})
smach.StateMachine.add('CHECK_IF_ENTITY_FOUND',
CheckIfDescribedEntityAvailable(
robot=robot,
knowledge=knowledge,
description_designator=description_designator,
found_entity_designator=found_entity_designator.writeable,
candidate_entities_designator=segmented_entities_designator),
transitions={'succeeded': 'succeeded',
'failed': 'not_found'})
def __init__(self, robot, supporting_entity_designator, arm_designator):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'inspect_failed', 'grasp_failed'])
self._robot = robot
self._supporting_entity_designator = supporting_entity_designator
self._arm_designator = arm_designator
self._classification_result_designator = VariableDesignator(
[], resolve_type=[ClassificationResult])
self._grasp_entity_designator = VariableDesignator(resolve_type=Entity)
with self:
smach.StateMachine.add(
'INSPECT',
states.Inspect(
robot=self._robot,
entityDes=self._supporting_entity_designator,
objectIDsDes=self._classification_result_designator,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={
'done': 'SELECT_EASIEST_GRASP_ENTITY',
'failed': 'inspect_failed'
})
smach.StateMachine.add(
'SELECT_EASIEST_GRASP_ENTITY',
SelectEasiestGraspEntity(robot=self._robot,
classification_result_designator=self.
_classification_result_designator,
grasp_entity_designator=self.
_grasp_entity_designator.writeable),
transitions={
'selected': 'GRAB',
'failed': 'inspect_failed'
})
smach.StateMachine.add('GRAB',
states.Grab(
robot=self._robot,
item=self._grasp_entity_designator,
arm=self._arm_designator),
transitions={
'done': 'succeeded',
'failed': 'grasp_failed'
})
def __init__(self, robot, source_entity_designator, area_name_designator, navigation_area_designator,
found_entity_designator):
StateMachine.__init__(self, outcomes=['found', 'not_found'])
segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult])
with self:
StateMachine.add('SAY_LOOKING', Say(robot, ["I'm going to look for the cup"], block=False),
transitions={'spoken': 'INSPECT_SOURCE_ENTITY'})
StateMachine.add('INSPECT_SOURCE_ENTITY', Inspect(robot=robot,
entityDes=source_entity_designator,
objectIDsDes=segmented_entities_designator,
searchArea=area_name_designator,
navigation_area=navigation_area_designator),
transitions={'done': 'GET_CUP_ID',
'failed': 'not_found'})
StateMachine.add('GET_CUP_ID', GetCupId(robot=robot,
found_entity_designator=found_entity_designator.writeable,
candidate_entities_designator=segmented_entities_designator),
transitions={'succeeded': 'found',
'failed': 'not_found'})
# import states from another file
import test_states as test_states
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# INITIALIZATIONS
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# load knowledge
common_knowledge = load_knowledge("common")
challenge_knowledge = load_knowledge("challenge_test")
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints(
"[Challenge Test] ")
personNameDes = VariableDesignator("", resolve_type=str)
objectsIDsDes = VariableDesignator([], resolve_type=[ClassificationResult])
containerResultDes = VariableDesignator(0, resolve_type=int)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# NAVIGATION_CONTAINER
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
class EnterRoomContainer(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
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 __init__(self, robot, location_des):
"""
Visit all selected locations from the list, and handle the found objects
:param location_des: is a designator resolving to a dictionary with fields ... TODO
"""
smach.StateMachine.__init__(self, outcomes=['done'])
segment_areas = ValueByKeyDesignator(location_des,
"segment_areas", [str],
name='segment_areas')
segment_area = VariableDesignator(resolve_type=str,
name='segment_area')
navigation_area_des = ValueByKeyDesignator(location_des,
key='navigation_area',
resolve_type=str,
name='navigation_area')
location_id_des = ValueByKeyDesignator(location_des,
key='name',
resolve_type=str,
name='location_id')
room_des = EdEntityDesignator(robot,
id_designator=ValueByKeyDesignator(
location_des,
key="room",
resolve_type=str))
# Set up the designators for this machine
e_classifications_des = VariableDesignator(
[],
resolve_type=[ClassificationResult],
name='e_classifications_des')
e_des = EdEntityDesignator(robot,
id_designator=location_id_des,
name='e_des')
with self:
smach.StateMachine.add('ITERATE_NEXT_AREA',
IterateDesignator(segment_areas,
segment_area.writeable),
transitions={
"next": 'INSPECT',
"stop_iteration": "done"
})
# Segment the area and handle segmented objects for the specified navigation area
# for i, segment_area in enumerate(segment_areas):
smach.StateMachine.add(
'INSPECT',
NavigateToSymbolic(robot, {e_des: navigation_area_des}, e_des),
transitions={
'arrived': 'SEGMENT_SAY',
'unreachable': "SAY_UNREACHABLE",
'goal_not_defined': "SAY_UNREACHABLE"
})
smach.StateMachine.add("SEGMENT_SAY",
Say(robot,
"Looking {area} the {entity}",
area=segment_area,
entity=location_id_des,
block=False),
transitions={"spoken": "RISE"})
smach.StateMachine.add('RISE',
RiseForInspect(robot, e_des, segment_area),
transitions={
'succeeded': 'SEGMENT',
'failed': 'SEGMENT'
})
smach.StateMachine.add(
'SEGMENT',
SegmentObjects(robot, e_classifications_des.writeable, e_des,
segment_area),
transitions={'done': "HANDLE_DETECTED_ENTITIES"})
smach.StateMachine.add("SAY_UNREACHABLE",
Say(robot,
"I failed to inspect the {furn}",
furn=location_id_des,
block=True),
transitions={"spoken": "done"})
smach.StateMachine.add("HANDLE_DETECTED_ENTITIES",
HandleDetectedEntities(
robot, e_classifications_des,
location_id_des, segment_area,
room_des),
transitions={"done": "ITERATE_NEXT_AREA"})
robot.get_arm().send_joint_goal("reset")
# Fill the designator and user data the furniture inspection
furniture_designator.write(
robot.ed.get_entity(final_result.entity_id))
user_data['laser_dot'] = result.intersection_point
return 'done'
with self:
self.add('PREPARE_OPERATOR',
CBState(_prepare_operator),
transitions={'done': 'GET_OPERATOR'})
self.add('GET_OPERATOR',
CBState(_get_operator),
transitions={'done': 'GET_FURNITURE'})
self.add('GET_FURNITURE',
CBState(_get_furniture),
transitions={
'done': 'done',
'failed': 'GET_OPERATOR'
})
if __name__ == '__main__':
rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
furniture_designator = VariableDesignator(resolve_type=Entity)
hero = Hero()
hero.reset()
while not rospy.is_shutdown():
GetFurnitureFromOperatorPose(hero,
furniture_designator.writeable).execute()
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 __init__(self,
robot,
ask_follow=True,
learn_face=True,
operator_radius=1,
lookat_radius=1.2,
timeout=1.0,
start_timeout=10,
operator_timeout=20,
distance_threshold=None,
lost_timeout=60,
lost_distance=0.8,
operator_id_des=VariableDesignator(resolve_type=str),
standing_still_timeout=20,
operator_standing_still_timeout=3.0,
replan=False):
""" Constructor
:param robot: robot object
:param ask_follow:
:param learn_face:
:param operator_radius:
:param lookat_radius:
:param timeout:
:param start_timeout:
:param operator_timeout:
:param distance_threshold:
:param lost_timeout: How long to look for the operator when we lost him/her?
:param lost_distance:
:param operator_id_des:
:param standing_still_timeout:
:param operator_standing_still_timeout:
:param replan:
"""
smach.State.__init__(
self, outcomes=["stopped", 'lost_operator', "no_operator"])
self._robot = robot
self._time_started = None
self._operator = None
self._operator_id = None
self._operator_name = "operator"
self._operator_radius = operator_radius
self._lookat_radius = lookat_radius
self._start_timeout = start_timeout
self._breadcrumbs = [] # List of Entity's
self._breadcrumb_distance = 0.1 # meters between dropped breadcrumbs
self._operator_timeout = operator_timeout
self._ask_follow = ask_follow
self._learn_face = learn_face
self._lost_timeout = lost_timeout
self._lost_distance = lost_distance
self._standing_still_timeout = standing_still_timeout
self._operator_standing_still_timeout = operator_standing_still_timeout
self._operator_id_des = operator_id_des
self._operator_distance = None
self._operator_pub = rospy.Publisher(
'/%s/follow_operator/operator_position' % robot.robot_name,
geometry_msgs.msg.PointStamped,
queue_size=10)
self._plan_marker_pub = rospy.Publisher(
'/%s/global_planner/visualization/markers/global_plan' %
robot.robot_name,
Marker,
queue_size=10)
self._breadcrumb_pub = rospy.Publisher(
'/%s/follow_operator/breadcrumbs' % robot.robot_name,
Marker,
queue_size=10)
self._face_pos_pub = rospy.Publisher(
'/%s/follow_operator/operator_detected_face' % robot.robot_name,
geometry_msgs.msg.PointStamped,
queue_size=10)
self._last_pose_stamped = None
self._last_pose_stamped_time = None
self._last_operator_fs = None
self._replan_active = False
self._last_operator = None
self._replan_allowed = replan
self._replan_timeout = 15 # seconds before another replan is allowed
self._replan_time = rospy.Time.now() - rospy.Duration(
self._replan_timeout)
self._replan_attempts = 0
self._max_replan_attempts = 3
self._period = 0.5
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 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, 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'}
)