def __init__(self, robot, arm_designator, drop_bag_pose):
"""
:param robot: the robot with which to execute this state machine
:param arm_designator: ArmDesignator resolving to Arm holding the bag to drop
"""
smach.StateMachine.__init__(self, outcomes=['done'])
check_type(arm_designator, Arm)
with self:
smach.StateMachine.add('DROP_POSE',
states.ArmToJointConfig(
robot, arm_designator, drop_bag_pose),
transitions={
'succeeded': 'OPEN_AFTER_DROP',
'failed': 'OPEN_AFTER_DROP'
})
smach.StateMachine.add('OPEN_AFTER_DROP',
states.SetGripper(
robot,
arm_designator,
gripperstate=GripperState.OPEN),
transitions={
'succeeded': 'RESET_ARM',
'failed': 'RESET_ARM'
})
smach.StateMachine.add('RESET_ARM',
states.ResetArms(robot),
transitions={'done': 'done'})
def __init__(self, robot, arm_designator):
"""
:param robot: the robot with which to execute this state machine
:param arm_designator: ArmDesignator resolving to Arm holding the bag to drop
"""
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
check_type(arm_designator, Arm)
with self:
smach.StateMachine.add( 'OPEN_BEFORE_DROP', states.SetGripper(robot, arm_designator, gripperstate='open'),
transitions={'succeeded' : 'DROP_POSE',
'failed' : 'DROP_POSE'})
smach.StateMachine.add("DROP_POSE", states.ArmToJointConfig(robot, arm_designator, "drop_bag_pose"),
transitions={'succeeded' :'RESET_ARM_OK',
'failed' :'RESET_ARM_FAIL'})
smach.StateMachine.add( 'RESET_ARM_OK', states.ResetArms(robot),
transitions={'done' : 'succeeded'})
smach.StateMachine.add( 'RESET_ARM_FAIL', states.ResetArms(robot),
transitions={'done' : 'failed'})
def __init__(self, robot):
# type: (Robot) -> str
"""
Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des')
room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des')
objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des')
unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm')
with self:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot=robot),
transitions={"initialized": "INITIAL_POSE",
"abort": "aborted"})
smach.StateMachine.add("INITIAL_POSE",
states.SetInitialPose(robot,
challenge_knowledge.starting_point),
transitions={"done": "INSPECT_BAR",
"preempted": "aborted",
"error": "INSPECT_BAR"})
# Inspect bar and store the list of available drinks
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot,
entityDes=bar_designator,
navigation_area="in_front_of",
objectIDsDes=objects_list_des),
transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019
"failed": "INSPECT_FALLBACK"})
smach.StateMachine.add("CHECK_INSPECT_RESULT",
CheckInspect(objects_list_des,
[ClassificationResult]),
transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK",
"false": "INSPECT_FALLBACK"})
smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK",
IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects,
classification_list_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des.writeable,
max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS),
transitions={"done": "NAVIGATE_TO_ROOM",
"failed": "INSPECT_FALLBACK"})
# Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink
smach.StateMachine.add("INSPECT_FALLBACK",
AskAvailability(robot=robot,
unavailable_drink_designator=unav_drink_des.writeable,
objects=common_knowledge.objects),
transitions={"succeeded": "RESET_ROBOT",
"failed": "RESET_ROBOT"})
smach.StateMachine.add("RESET_ROBOT",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "NAVIGATE_TO_ROOM",
'failed': "NAVIGATE_TO_ROOM"})
# Navigate to the predefined room
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(robot=robot, entity_designator_room=room_designator),
transitions={"arrived": "SAY_HI",
"unreachable": "SAY_HI",
"goal_not_defined": "aborted"})
smach.StateMachine.add("SAY_HI",
states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)),
transitions={"spoken": "SERVE_DRINK_1"})
# Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state
# is constructed every time
for idx in range(1, challenge_knowledge.NR_DRINKS + 1):
next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE"
smach.StateMachine.add("SERVE_DRINK_{}".format(idx),
ServeOneDrink(robot=robot,
bar_designator=bar_designator,
room_id=challenge_knowledge.room_id,
room_designator=room_designator,
objects_list_des=objects_list_des,
unav_drink_des=unav_drink_des,
name_options=common_knowledge.names,
objects=common_knowledge.objects),
transitions={"succeeded": next_state,
"failed": next_state,
"aborted": next_state})
smach.StateMachine.add("SAY_DONE",
states.Say(robot, "My job here is done. Enjoy your day and see you next time"),
transitions={"spoken": "succeeded"})
def setup_statemachine(robot):
place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name")
place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot,
place_name,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked
# For this challenge, unlocking is not needed
bag_arm_designator = empty_arm_designator.lockable()
bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
current_item = ds.VariableDesignator(Entity("dummy",
"dummy",
"/{}/base_link".format(robot.robot_name),
kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5),
None,
{},
[],
datetime.datetime.now()),
name="current_item")
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(robot, challenge_knowledge.starting_point),
transitions={'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'})
# TODO: learn operator state needs to be added before follow
# smach.StateMachine.add('WAIT_TO_FOLLOW',
# WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']),
# transitions={'success': 'FOLLOW_OPERATOR',
# 'abort': 'Aborted'})
smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER',
states.Say(robot, ["Are we at the car or should I follow you?"], block=True),
transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER',
WaitForOperatorCommand(robot,
possible_commands=[
"follow",
'follow me',
"here is the car",
"stop following",
"stop following me",
],
commands_as_outcomes=True),
transitions={'follow': 'FOLLOW_OPERATOR',
'follow me': 'FOLLOW_OPERATOR',
'here is the car': 'REMEMBER_CAR_LOCATION',
'stop following': 'REMEMBER_CAR_LOCATION',
'stop following me': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={'stopped': 'ASK_FOLLOW_OR_REMEMBER',
'lost_operator': 'ASK_FOLLOW_OR_REMEMBER',
'no_operator': 'ASK_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
StoreCarWaypoint(robot),
transitions={'success': 'ASK_DESTINATION',
'abort': 'Aborted'})
smach.StateMachine.add('ASK_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"], block=True),
transitions={'spoken': 'WAIT_FOR_DESTINATION'})
smach.StateMachine.add('WAIT_FOR_DESTINATION',
WaitForOperatorCommand(robot,
possible_commands=challenge_knowledge.waypoints.keys(),
commands_as_userdata=True),
transitions={'success': 'GRAB_ITEM',
'abort': 'Aborted'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add('GRAB_ITEM',
GrabItem(robot, bag_arm_designator, current_item),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes.
'failed': 'BACKUP_CLOSE_GRIPPER'},
remapping={'target_room_in': 'command_recognized',
'target_room_out': 'target_room'})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(robot, bag_arm_designator, 'driving_bag_pose'),
transitions={'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add('SAY_GOING_TO_ROOM',
states.Say(robot, ["Let me bring in your groceries",
"Helping you carry stuff",
"I'm going back inside"],
block=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add('GOTO_DESTINATION',
NavigateToRoom(robot),
transitions={'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
DropBagOnGround(robot, bag_arm_designator),
transitions={'succeeded': 'ASKING_FOR_HELP',
'failed': 'ASKING_FOR_HELP'})
smach.StateMachine.add('ASKING_FOR_HELP',
#TODO: look and then face new operator
states.Say(robot, "Please follow me and help me carry groceries into the house"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('GOTO_CAR',
states.NavigateToWaypoint(robot,
ds.EntityByIdDesignator(robot,
id=challenge_knowledge.waypoint_car['id']),
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'})
smach.StateMachine.add('OPEN_DOOR',
#TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('AT_END',
states.Say(robot, ["We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"],
block=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "help_me_carry")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
msg = "\n".join([
"==============================================",
"== CHALLENGE HELP ME CARRY ==",
"=============================================="
])
rospy.loginfo("\n" + msg)
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_goals": [
"handover_to_human", "reset",
challenge_knowledge.driving_bag_pose,
challenge_knowledge.drop_bag_pose
],
"required_gripper_types": [GripperTypes.GRASPING]
},
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
self.place_position = ds.LockingDesignator(EmptySpotDesignator(
robot,
self.target_destination,
arm_designator=self.bag_arm_designator,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={
'stopped': 'ASK_FOR_TASK',
'lost_operator': 'ASK_FOR_TASK',
'no_operator': 'ASK_FOR_TASK'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add(
'WAIT_FOR_TASK',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.commands.keys(),
commands_as_outcomes=True),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'
})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
states.HandoverFromHuman(
robot,
self.bag_arm_designator,
"current_item",
self.current_item,
arm_configuration=challenge_knowledge.carrying_bag_pose),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER',
# For now in simulation timeout is considered a success.
'failed': 'BACKUP_CLOSE_GRIPPER'
})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(
robot,
self.bag_arm_designator,
gripperstate=GripperState.CLOSE),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'
})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(
robot, self.bag_arm_designator,
challenge_knowledge.driving_bag_pose),
transitions={
'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'
})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.default_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.backup_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
hmc_states.DropBagOnGround(
robot, self.bag_arm_designator,
challenge_knowledge.drop_bag_pose),
transitions={'done': 'ASKING_FOR_HELP'})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, bar_designator, room_id, room_designator,
objects_list_des, unav_drink_des, name_options, objects):
"""
Initialization method
:param robot: robot api object
:param bar_designator: (EntityDesignator) in which the bar location is stored
:param room_id: room ID from challenge knowledge
:param room_designator: (EntityDesignator) in which the room location is stored
:param objects_list_des: (VariableDesignator) in which the available drinks are stored
:param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored
:param name_options: Names from common knowledge
:param objects: Objects from common knowledge
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
arm_designator = ds.UnoccupiedArmDesignator(robot=robot,
arm_properties={},
name='arm_des').lockable()
drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des')
drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des')
operator_name = ds.VariableDesignator(resolve_type=str, name='name_des')
operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des')
learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2')
with self:
# Lock the arm_designator
smach.StateMachine.add("LOCK_ARM",
states.LockDesignator(arm_designator),
transitions={'locked': "GET_ORDER"})
# Get order
smach.StateMachine.add("GET_ORDER",
GetOrder(robot=robot,
operator_name=operator_name,
drink_designator=drink_str_designator,
available_drinks_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des,
name_options=name_options,
objects=objects,
learn_check_designator=learn_check_designator.writeable,
target_room_designator=room_designator),
transitions={"succeeded": "INSPECT_BAR",
"failed": "failed",
"aborted": "aborted"})
# Inspect bar
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"),
transitions={"done": "GRASP_DRINK",
"failed": "FALLBACK_BAR"})
# Grasp drink
smach.StateMachine.add("GRASP_DRINK",
states.Grab(robot=robot, item=drink_designator, arm=arm_designator),
transitions={"done": "FIND_OPERATOR",
"failed": "FALLBACK_BAR"})
# Inspect or grasp fallback - ask for assistance
smach.StateMachine.add("FALLBACK_BAR",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "HANDOVER_FROM_HUMAN"})
# Handover from human fallback
smach.StateMachine.add("HANDOVER_FROM_HUMAN",
states.HandoverFromHuman(robot=robot, arm_designator=arm_designator,
grabbed_entity_designator=drink_designator),
transitions={"succeeded": "RESET_ROBOT_2",
"failed": "RESET_ROBOT_2",
"timeout": "RESET_ROBOT_2"})
smach.StateMachine.add("RESET_ROBOT_2",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "CHECK_LEARN_OPERATOR",
'failed': "CHECK_LEARN_OPERATOR"})
smach.StateMachine.add("CHECK_LEARN_OPERATOR",
states.CheckBool(learn_check_designator),
transitions={"true": "FIND_OPERATOR",
"false": "GO_TO_ROOM"})
smach.StateMachine.add("GO_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "SAY_NOT_FOUND",
"unreachable": "failed",
"goal_not_defined": "aborted"})
# Find operator
smach.StateMachine.add("FIND_OPERATOR",
states.FindPersonInRoom(robot=robot,
area=room_id,
name=operator_name,
discard_other_labels=True,
found_entity_designator=operator_designator.writeable),
transitions={"found": "GOTO_OPERATOR",
"not_found": "SAY_NOT_FOUND"})
# Move to this person
smach.StateMachine.add("GOTO_OPERATOR",
states.NavigateToObserve(robot=robot,
entity_designator=operator_designator),
transitions={"arrived": "SAY_THE_NAME",
"unreachable": "SAY_NOT_FOUND",
"goal_not_defined": "SAY_NOT_FOUND"})
# Say not found
smach.StateMachine.add("SAY_NOT_FOUND",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("not_found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
# Say the name
smach.StateMachine.add("SAY_THE_NAME",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
smach.StateMachine.add("RISE_FOR_HMI_2",
states.RiseForHMI(robot=robot),
transitions={"succeeded": "HAND_OVER",
"failed": "HAND_OVER"})
# Hand over the drink to the operator
smach.StateMachine.add("HAND_OVER",
states.HandoverToHuman(robot=robot,
arm_designator=arm_designator),
transitions={"succeeded": "UNLOCK_ARM",
"failed": "UNLOCK_ARM"})
smach.StateMachine.add("UNLOCK_ARM",
states.UnlockDesignator(arm_designator),
transitions={'unlocked': "RESET_ROBOT_3"})
smach.StateMachine.add("RESET_ROBOT_3",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "RETURN_TO_ROOM",
'failed': "RETURN_TO_ROOM"})
smach.StateMachine.add("RETURN_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "aborted"})
def __init__(self, robot, operator_name, drink_designator,
available_drinks_designator, unavailable_drink_designator,
name_options, objects, learn_check_designator, target_room_designator):
# type: (Robot, str, VariableDesignator) -> None
"""
Initialization method
:param robot: robot api object
:param operator_name: name with which the operator will be stored in image recognition module
:param drink_designator: (VariableDesignator) in which the drink to fetch is stored
:param available_drinks_designator: (VariableDesignator) in which the available drinks are stored
:param unavailable_drink_designator: (VariableDesignator) in which the unavailable drink is stored
:param name_options: Names from common knowledge
:param objects: Objects from common knowledge
:param learn_check_designator: (VariableDesignator) Bool flag indicating whether the operator was learnt
successfully
:param target_room_designator: (EdEntityDesignator) Entity specifying the target room where the operator needs
to be searched for getting an order
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_3')
with self:
# Operator id
caller_id = "operator"
caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name="caller_des", none_resolve=True)
smach.StateMachine.add("RESET_ROBOT_GET_ORDER",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "SAY_PEOPLE_WITHOUT_DRINKS",
'failed': "SAY_PEOPLE_WITHOUT_DRINKS"})
# Detect - people holding drinks and people without drinks
smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS",
states.Say(robot=robot, sentence="Trying to find people without a drink",
look_at_standing_person=True,
block=True),
transitions={"spoken": "FIND_PERSON_WITHOUT_DRINK"})
# TODO: Change DummyState to actual state
smach.StateMachine.add("FIND_PERSON_WITHOUT_DRINK",
states.SetPoseFirstFoundPersonToEntity(robot=robot,
properties={'tags': ['LNotHolding', 'RNotHolding']},
strict=True,
dst_entity_designator=caller_id,
query_entity_designator=target_room_designator),
transitions={"done": "SAY_I_HAVE_SEEN",
"failed": "SAY_PEOPLE_WITHOUT_DRINKS_FAILED"})
# Detect fallback - detect waving people
smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS_FAILED",
states.Say(robot=robot,
sentence="Could not detect people without drinks",
look_at_standing_person=True,
block=True),
transitions={"spoken": "ASK_FOR_WAVING"})
smach.StateMachine.add("ASK_FOR_WAVING",
states.Say(robot=robot,
sentence="Please raise your arm completely and wave, if you want me to bring you something",
look_at_standing_person=True,
block=True),
transitions={"spoken": "WAIT_FOR_WAVING"}) # Change to WAIT_FOR_WAVING
smach.StateMachine.add("WAIT_FOR_WAVING",
states.SetPoseFirstFoundPersonToEntity(robot=robot,
properties={'tags': ['LWave', 'RWave']},
strict=False,
dst_entity_designator=caller_id,
query_entity_designator=target_room_designator),
transitions={"done": "SAY_I_HAVE_SEEN",
"failed": "SAY_COULD_NOT_FIND_WAVING"})
# Navigate to person who wants to place an order
smach.StateMachine.add("SAY_COULD_NOT_FIND_WAVING",
states.Say(robot=robot,
sentence="I did not find any waving person.",
look_at_standing_person=True,
block=True),
transitions={"spoken": "ASK_STEP_IN_FRONT"})
smach.StateMachine.add("SAY_I_HAVE_SEEN",
states.Say(robot=robot,
sentence="Found person who might want to place an order. I will be there shortly!",
look_at_standing_person=True,
block=True),
transitions={"spoken": "NAVIGATE_TO_PERSON"})
# Navigate to waving people
smach.StateMachine.add("NAVIGATE_TO_PERSON",
states.NavigateToObserve(robot=robot, entity_designator=caller_designator,
radius=1.1),
transitions={"arrived": "LEARN_NAME",
"unreachable": "SAY_COULD_NOT_NAVIGATE",
"goal_not_defined": "SAY_PEOPLE_WITHOUT_DRINKS"})
# Detect waving people fallback - ask operator in front
smach.StateMachine.add("SAY_COULD_NOT_NAVIGATE",
states.Say(robot=robot,
sentence="Sorry! I could not navigate to you.",
look_at_standing_person=True),
transitions={"spoken": "ASK_STEP_IN_FRONT"})
smach.StateMachine.add("ASK_STEP_IN_FRONT",
states.Say(robot=robot,
sentence="Please step in front of me to give your order",
look_at_standing_person=True,
block=True),
transitions={"spoken": "LEARN_NAME"})
# Ask operator for his name
smach.StateMachine.add("LEARN_NAME",
states.AskPersonName(robot=robot,
person_name_des=operator_name.writeable,
name_options=name_options,
default_name="john",
nr_tries=2),
transitions={"succeeded": "LEARN_OPERATOR",
"failed": "LEARN_NAME_FALLBACK",
"timeout": "LEARN_NAME_FALLBACK"})
# Ask operator for his name fallback
smach.StateMachine.add("LEARN_NAME_FALLBACK",
states.Say(robot=robot,
sentence="Sorry, I did not get your name, I'll just call you john",
look_at_standing_person=True),
transitions={"spoken": "LEARN_OPERATOR"})
# Learn operator
smach.StateMachine.add("LEARN_OPERATOR",
states.LearnPerson(robot=robot,
name_designator=operator_name,
nr_tries=5),
transitions={"succeeded": "ASK_DRINK",
"failed": "LEARN_OPERATOR_FALLBACK"})
# Learn operator fallback
smach.StateMachine.add("LEARN_OPERATOR_FALLBACK",
states.Say(robot=robot,
sentence="I will call you by your name when I'm back",
look_at_standing_person=True,
block=True),
transitions={"spoken": "LEARN_OPERATOR_FLAG_TOGGLE"})
smach.StateMachine.add("LEARN_OPERATOR_FLAG_TOGGLE",
states.ToggleBool(learn_check_designator),
transitions={"done": "ASK_DRINK"})
# Ask for preferred beverage
smach.StateMachine.add("ASK_DRINK",
AskDrink(robot=robot,
operator_name=operator_name,
drink_designator=drink_designator.writeable,
available_drinks_designator=available_drinks_designator,
unavailable_drink_designator=unavailable_drink_designator,
objects=objects),
transitions={"succeeded": "succeeded",
"failed": "failed",
"aborted": "aborted"})
def __init__(self, robot, trashbin_designator, arm_designator):
"""
:param robot: robot object
:param trashbin_designator: EdEntityDesignator designating the trashbin
:param arm_designator: arm designator resolving to the arm with which to grab
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
place_pose_designator = EmptySpotDesignator(robot, trashbin_designator, arm_designator)
with self:
# @cb_interface(outcomes=['done'])
# def _joint_goal(_):
# arm = arm_designator.resolve()
# if not arm:
# rospy.logerr("Could not resolve arm")
# return "failed"
# # Send to grab trash pose
# arm.send_joint_goal('grab_trash_bag')
# arm.wait_for_motion_done()
# return 'done'
#
# smach.StateMachine.add("JOINT_GOAL",
# CBState(_joint_goal),
# transitions={'done': 'GO_BIN'})
smach.StateMachine.add("GO_BIN",
states.NavigateToPlace(robot=robot, place_pose_designator=place_pose_designator,
arm_designator=arm_designator),
transitions={"arrived": "GET_BIN_POSITION",
"goal_not_defined": "aborted",
"unreachable": "failed"})
smach.StateMachine.add("GET_BIN_POSITION", GetTrashBin(robot=robot, trashbin=trashbin_designator),
transitions={"succeeded": "JOINT_PATH",
"failed": "failed"})
@cb_interface(outcomes=['done'])
def _joint_path(_):
robot.head.cancel_goal()
arm = arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed" # ToDo: fix
# Send to grab trash pose
arm._arm._send_joint_trajectory(
[[0.01, 0.0, -1.57, -1.57, 0.0],
[0.69, 0.0, -1.57, -1.57, 0.0],
[0.65, -2.2, -1.57, -1.57, 0.],
[0.65, -2.2, 0.0, -0.85, 0.]
]
)
arm.wait_for_motion_done()
return 'done'
smach.StateMachine.add("JOINT_PATH",
CBState(_joint_path),
transitions={'done': 'GO_TO_NEW_BIN'})
smach.StateMachine.add("GO_TO_NEW_BIN",
ControlToTrashBin(robot=robot, trashbin_id=trashbin_designator.id, radius=0.4,
yaw_offset=-0.2),
transitions={"done": "PREPARE_AND_GRAB"})
smach.StateMachine.add("PREPARE_AND_GRAB", GrabTrash(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "ANNOUNCE_PICKUP_FAIL"})
smach.StateMachine.add("ANNOUNCE_PICKUP_FAIL",
states.Say(robot, "Unfortunately I could not pick up the trash myself, let's go to"
"plan B!",
block=False),
transitions={'spoken': 'ASK_HANDOVER'})
# Ask human to handover the trash bag
smach.StateMachine.add("ASK_HANDOVER", HandoverFromHumanFigure(robot=robot, arm_designator=arm_designator,
grabbed_entity_label='thrash'),
transitions={"succeeded": "LOWER_ARM",
"failed": "failed",
"timeout": "TIMEOUT"})
arm_occupied_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={"required_goals": ["reset"
]})
smach.StateMachine.add("LOWER_ARM", states.ArmToJointConfig(robot=robot,
arm_designator=arm_occupied_designator,
configuration="reset"),
transitions={"succeeded": "RECEIVED_TRASH_BAG",
"failed": "RECEIVED_TRASH_BAG"})
smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw"
" it away, please move away.", block=True),
transitions={'spoken': 'succeeded'})
smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue",
block=False),
transitions={'spoken': "failed"})
def __init__(self, robot, selected_entity_designator, room_des):
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
store_entity_id_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id")
store_entity_des = ds.EdEntityDesignator(robot, id_designator=store_entity_id_des)
selected_entity_type_des = ds.AttrDesignator(selected_entity_designator, "type", resolve_type=str)
store_area_name_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id")
trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6, "drop_pose")
category_des = ds.VariableDesignator(resolve_type=str, name="category_des")
with self:
smach.StateMachine.add("SPEAK", robot_smach_states.Say(robot, ["I will pick-up the {object}",
"Let's move the {object}"],
object=selected_entity_type_des,
block=True),
transitions={"spoken": "GRAB"})
smach.StateMachine.add("GRAB",
robot_smach_states.Grab(robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(robot, {},
name="empty_arm_designator")),
transitions={"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"})
smach.StateMachine.add("ARM_RESET", robot_smach_states.ArmToJointConfig(robot,
ds.UnoccupiedArmDesignator(robot,
{},
name="empty_arm_designator"),
"reset"),
transitions={"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"})
smach.StateMachine.add('SAY_GRAB_SUCCESS',
robot_smach_states.Say(robot, ["Now I am going to move this item",
"Let's clean up this object",
"Away with this one",
"Everything will be cleaned"], block=False),
transitions={"spoken": "GET_CATEGORY"})
smach.StateMachine.add('SAY_GRAB_FAILED',
robot_smach_states.Say(robot, ["I could not grab the item.",
"I failed to grasp the item",
"I cannot reach the item",
"Item grab failed"], block=False),
transitions={"spoken": "failed"})
smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot),
transitions={"yes": "done",
"no": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot),
transitions={"yes": "GET_CATEGORY",
"no": "done"})
# # ROBOT
# smach.StateMachine.add('GET_CATEGORY',
# EntityToCategory(robot, selected_entity_designator, category_des.writeable),
# transitions={"done": "DETERMINE_PLACE_LOCATION",
# "failed": "NAVIGATE_TO_TRASH"})
# OPERATOR
smach.StateMachine.add('GET_CATEGORY', OperatorToCategory(robot, category_des.writeable, room_des),
transitions={"done": "DETERMINE_PLACE_LOCATION",
"failed": "NAVIGATE_TO_TRASH"})
smach.StateMachine.add('DETERMINE_PLACE_LOCATION',
CategoryToLocation(category_des, store_entity_id_des.writeable,
store_area_name_des.writeable),
transitions={"trashbin": "INSPECT_TRASH",
"other": "PLACE_TO_STORE",
"failed": "NAVIGATE_TO_TRASH"})
smach.StateMachine.add('NAVIGATE_TO_TRASH',
robot_smach_states.NavigateToPlace(robot, trash_place_pose,
ds.OccupiedArmDesignator(robot, {},
name="occupied_arm_designator")),
transitions={"arrived": "PLACE_IN_TRASH",
"unreachable": "SAY_PLACE_FAILED",
"goal_not_defined": "SAY_PLACE_FAILED"})
smach.StateMachine.add('INSPECT_TRASH',
robot_smach_states.Inspect(robot,
store_entity_des),
transitions={"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('PLACE_IN_TRASH',
robot_smach_states.Place(robot,
selected_entity_designator,
trash_place_pose,
ds.OccupiedArmDesignator(robot, {},
name="occupied_arm_designator")),
transitions={"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('PLACE_TO_STORE', robot_smach_states.Place(robot, selected_entity_designator,
store_entity_des,
ds.OccupiedArmDesignator(robot, {}, name=
"occupied_arm_designator"),
"on_top_of"),
transitions={"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('SAY_PLACE_SUCCESS',
robot_smach_states.Say(robot, ["Bye bye!",
"Yeah!",
"Successfully disposed the item",
"Another score for HERO"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('SAY_PLACE_FAILED',
robot_smach_states.Say(robot, ["I could not cleanup the item.",
"I cannot put the item in the trashbin",
"Item cleanup failed"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})