def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(
self._robot, {
"required_goals": ["reset", "handover_to_human"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
place = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self.place_designator,
arm=arm_designator)
result = place.execute()
# If failed, do handover to human in order to continue
if result != "done":
rospy.loginfo("{place} resulted in {out}".format(place=place,
out=result))
handover = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
handover.execute()
return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(
self._robot,
arm_properties={
"required_trajectories": ["prepare_place"],
"required_goals": ["reset", "handover_to_human"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None):
arm = None
# See if there's an arm holding something
for k, v in self._robot.arms.iteritems():
if v.occupied_by is not None:
arm = v
break
if arm is None:
return "failed"
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.ArmDesignator(all_arms={arm.side: arm},
preferred_arm=arm)
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def __init__(self, robot, place_designator=None):
""" Constructor
:param robot: robot object
:param place_designator: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator
"""
smach.State.__init__(self, outcomes=["succeeded", "failed"])
self._robot = robot
if place_designator is not None:
self._place_designator = place_designator
else:
place_entity_designator = ds.EdEntityDesignator(
robot=robot, id=DEFAULT_PLACE_ENTITY)
self._place_designator = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=place_entity_designator,
area=DEFAULT_PLACE_AREA)
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
furniture_designator = VariableDesignator(resolve_type=Entity)
entity_designator = VariableDesignator(resolve_type=Entity)
arm_designator = UnoccupiedArmDesignator(robot, {})
with state_machine:
# Intro
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT),
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
# Say we're gonna start
StateMachine.add('SAY_START', Say(robot, "Hand me that it is!", block=False),
transitions={'spoken': 'NAVIGATE_TO_START'})
# Drive to the start location
StateMachine.add('NAVIGATE_TO_START',
NavigateToWaypoint(robot, ds.EdEntityDesignator(robot, id=HOME_LOCATION)),
transitions={'arrived': 'GET_FURNITURE_FROM_OPERATOR_POSE',
'unreachable': 'NAVIGATE_TO_START', # ToDo: other fallback
'goal_not_defined': 'done'}) # I'm not even going to fill this in
# The pre-work
StateMachine.add('GET_FURNITURE_FROM_OPERATOR_POSE',
GetFurnitureFromOperatorPose(robot, furniture_designator.writeable),
transitions={'done': 'INSPECT_FURNITURE'})
# Go to the furniture object that was pointing to see what's there
StateMachine.add('INSPECT_FURNITURE',
InspectFurniture(robot, furniture_designator, entity_designator.writeable),
transitions={"succeeded": "IDENTIFY_OBJECT",
"failed": "NAVIGATE_TO_START"}) # If no entities, try again
# Point at the object
StateMachine.add('IDENTIFY_OBJECT',
IdentifyObject(robot, entity_designator, arm_designator),
transitions={'done': 'NAVIGATE_TO_START', # Just keep on going
'failed': 'NAVIGATE_TO_START'}) # Just keep on going
return state_machine
def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(self._robot)
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def __init__(self, dummy_robot):
smach.StateMachine.__init__(self, outcomes=[
"succeeded",
"failed",
])
# Create designators
dummy_trashbin_designator = ds.EdEntityDesignator(
dummy_robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator')
dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {})
dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {})
with self:
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(dummy_robot,
dummy_trashbin_designator,
dummy_arm_designator_un),
transitions={
"succeeded": "TURN_BASE",
"failed": "TURN_BASE",
"aborted": "failed"
})
smach.StateMachine.add("TURN_BASE",
states.ForceDrive(dummy_robot, 0, 0, 0.5,
3),
transitions={"done": "DROP_TRASH"})
smach.StateMachine.add("DROP_TRASH",
DropTrash(dummy_robot,
dummy_arm_designator_oc),
transitions={
"succeeded": "succeeded",
"failed": "failed"
})
import argparse
import robot_smach_states.util.designators as ds
from robot_skills import get_robot
if __name__ == "__main__":
rospy.init_node("testdesignator")
parser = argparse.ArgumentParser(
description="Test the empty spot designator")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
robot = get_robot(args.robot)
furniture_designator = ds.EdEntityDesignator(robot, id="dinner_table")
def with_area():
esd = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=furniture_designator,
name="with_area",
area="on_top_of")
print(esd.resolve())
def without_area():
esd = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=furniture_designator,
name="without_area")
print(esd.resolve())
return 'succeeded'
if __name__ == "__main__":
from robot_skills import get_robot
if len(sys.argv) > 1:
robot_name = sys.argv[1]
point_at = sys.argv[2]
look_at = sys.argv[3]
rospy.init_node('test_follow_operator')
robot = get_robot(robot_name)
sm = PointAt(
robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=ds.EdEntityDesignator(robot,
id=point_at,
name='point_at_des'),
look_at_designator=ds.EdEntityDesignator(robot,
id=look_at,
name='look_at_des'))
sm.execute()
else:
print(
"Please provide robot name, point_at ID and look_at ID as argument."
)
exit(1)
def __init__(self, robot, attempts = 1, sleep_interval = 1):
# TODO: add center_point in front of the robot and radius of the search on ds.EdEntityDesignator
human_entity = ds.EdEntityDesignator(robot, type="human")
WaitForDesignator.__init__(self, robot, human_entity, attempts, sleep_interval)
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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
with self:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
transitions={"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
radius=0.5),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
radius=0.7),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'ANNOUNCEMENT',
'goal_not_defined': 'ANNOUNCEMENT'})
# Part I: Set a table
smach.StateMachine.add('ANNOUNCEMENT',
states.Say(robot, "Let's see if my master has a task for me! ", block=True),
transitions={'spoken': 'FETCH_COMMAND_I'})
smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0, "set"),
transitions={'done': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot, "What should I serve, master?", block=True),
transitions={'spoken': 'GET_ORDER'})
smach.StateMachine.add('GET_ORDER',
GetBreakfastOrder(robot, knowledge.options,
grasp_designator1,
grasp_designator2,
grasp_designator3,
timeout=15.0),
transitions={'done': 'SET_THE_TABLE'})
smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
ManipulateMachine(robot=robot,
grasp_designator1=grasp_designator1,
grasp_designator2=grasp_designator2,
grasp_designator3=grasp_designator3,
grasp_furniture_id1=knowledge.grasp_furniture_id1,
grasp_furniture_id2=knowledge.grasp_furniture_id2,
place_furniture_id=knowledge.place_furniture_id),
transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION',
'failed': 'RETURN_TO_START_2'})
smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION',
states.Say(robot, "The table is set! Moving to the meeting point for the next task.",
block=False),
transitions={'spoken': 'RETURN_TO_START_2'})
# Part II: Clean the table
smach.StateMachine.add('RETURN_TO_START_2',
states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3),
transitions={'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'})
smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0, "clear"),
transitions={'done': 'CLEAR_UP'})
smach.StateMachine.add('CLEAR_UP', # Clear the table
ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.place_furniture_id,
place_furniture_id1=knowledge.grasp_furniture_id1,
place_furniture_id2=knowledge.grasp_furniture_id2),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am done here"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
with self:
# Part I: Set a table
smach.StateMachine.add('ENTER_ROOM', # Enter the room
states.Initialize(robot),
transitions={'initialized': 'ANNOUNCEMENT',
'abort': 'Aborted'})
smach.StateMachine.add('ANNOUNCEMENT',
states.Say(robot, "Let's see if my master has a task for me! ", block=True),
transitions={'spoken': 'FETCH_COMMAND_I'})
smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0, "set"),
transitions={'done': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot, "What should I serve, master?", block=True),
transitions={'spoken': 'GET_ORDER'})
smach.StateMachine.add('GET_ORDER',
GetBreakfastOrder(robot, knowledge.options,
grasp_designator1,
grasp_designator2,
grasp_designator3,
timeout=15.0),
transitions={'done': 'SET_THE_TABLE'})
smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
ManipulateMachine(robot=robot,
grasp_designator1=grasp_designator1,
grasp_designator2=grasp_designator2,
grasp_designator3=grasp_designator3,
grasp_furniture_id1=knowledge.cupboard,
grasp_furniture_id3=knowledge.cupboard,
place_furniture_id=knowledge.table),
transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION',
'failed': 'RETURN_TO_START_2'})
smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION',
states.Say(robot, "The table is set! Moving to the meeting point for the next task.",
block=False),
transitions={'spoken': 'RETURN_TO_START_2'})
# Part II: Clean the table
smach.StateMachine.add('RETURN_TO_START_2',
states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3),
transitions={'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'})
smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0, "clear"),
transitions={'done': 'CLEAR_UP'})
smach.StateMachine.add('CLEAR_UP', # Clear the table
ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.table,
place_furniture_id1=knowledge.cupboard,
place_furniture_id3=knowledge.cupboard),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am done here"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def __init__(self, robot, attempts = 1, sleep_interval = 1):
# TODO: add center_point in front of the robot and radius of the search on ds.EdEntityDesignator
# human_entity = ds.EdEntityDesignator(robot, center_point=gm.PointStamped(x=1.0, frame_id="base_link"), radius=1, id="human")
human_entity = ds.EdEntityDesignator(robot, type="human")
ds.WaitForDesignator.__init__(self, robot, human_entity, attempts, sleep_interval)
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
information_point_designator = ds.EdEntityDesignator(
robot, id_designator=information_point_id_designator)
with self:
single_item = InformMachine(robot)
if START_ROBUST:
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
states.Say(
robot,
"Near which furniture object should I go to start guiding people?"
),
transitions={"spoken": "WAIT_WHERE_TO_GO"})
smach.StateMachine.add(
"WAIT_WHERE_TO_GO",
states.HearOptionsExtra(
robot=robot,
spec_designator=ds.Designator(
initial_value=START_GRAMMAR),
speech_result_designator=hmi_result_des.writeable),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "ASK_WHERE_TO_GO"
}) # ToDo: add fallbacks #option: STORE_STARTING_POSE
smach.StateMachine.add(
"ASK_CONFIRMATION",
states.Say(robot, [
"I hear that you would like me to start the tours at"
" the {place}, is this correct?"
],
place=information_point_id_designator,
block=True),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
states.HearOptions(
robot=robot, options=["yes", "no"]),
transitions={
"yes": "MOVE_OUT_OF_MY_WAY",
"no": "ASK_WHERE_TO_GO",
"no_result": "ASK_WHERE_TO_GO"
})
smach.StateMachine.add(
"MOVE_OUT_OF_MY_WAY",
states.Say(robot,
"Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
states.WaitTime(robot=robot,
waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "in_front_of"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable": "WAIT_NAV_BACKUP",
"goal_not_defined": "Aborted"
}) # If this happens: never mind
smach.StateMachine.add("WAIT_NAV_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "near"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable":
"SAY_CANNOT_REACH_WAYPOINT", # Current pose backup
"goal_not_defined": "Aborted"
}) # If this happens: never mind
@smach.cb_interface(outcomes=["done"])
def _turn_around(userdata=None):
""" Turns the robot approximately 180 degrees around """
v_th = 0.5
robot.base.force_drive(vx=0.0,
vy=0.0,
vth=v_th,
timeout=math.pi / v_th)
return "done"
smach.StateMachine.add(
"TURN_AROUND",
smach.CBState(_turn_around),
transitions={"done": "STORE_STARTING_POSE"})
smach.StateMachine.add(
"SAY_CANNOT_REACH_WAYPOINT",
states.Say(
robot, "I am not able to reach the starting point."
"I'll use this as starting point"),
transitions={"spoken": "STORE_STARTING_POSE"})
else:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot),
transitions={
"initialized":
"STORE_STARTING_POSE",
"abort": "Aborted"
})
## This is purely for a back up scenario until the range iterator
@smach.cb_interface(outcomes=["succeeded"])
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
smach.StateMachine.add("STORE_STARTING_POSE",
smach.CBState(store_pose),
transitions={"succeeded": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=["succeeded",
"failed"], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(1000),
it_label="index",
exhausted_outcome="succeeded")
with range_iterator:
smach.Iterator.set_contained_state(
"SINGLE_ITEM",
single_item,
loop_outcomes=["succeeded", "failed"])
smach.StateMachine.add("RANGE_ITERATOR", range_iterator, {
"succeeded": "AT_END",
"failed": "Aborted"
})
# End setup iterator
smach.StateMachine.add("AT_END",
states.Say(robot, "Goodbye"),
transitions={"spoken": "Done"})
def __init__(self,
robot,
grab_designator_1=None,
grab_designator_2=None,
place_designator=None,
pdf_writer=None):
"""
Constructor
:param robot: robot object
:param grab_designator_1: EdEntityDesignator designating the item to grab
:param grab_designator_2: EdEntityDesignator designating the item to grab
:param pdf_writer: WritePDF object to save images of recognized objects to pdf files
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.table_designator = ds.EntityByIdDesignator(
robot, id="temp") # will be updated later on
if grab_designator_1 is None:
grab_designator_1 = DefaultGrabDesignator(
robot=robot,
surface_designator=self.table_designator,
area_description=GRAB_SURFACE)
if grab_designator_2 is None:
grab_designator_2 = DefaultGrabDesignator(
robot=robot,
surface_designator=self.table_designator,
area_description=GRAB_SURFACE)
self.cabinet = ds.EntityByIdDesignator(
robot, id="temp") # will be updated later on
self.place_entity_designator = ds.EdEntityDesignator(robot=robot,
id="temp")
self.arm_designator = ds.ArmDesignator(robot, {})
self.place_designator = EmptySpotDesignator(
robot=robot,
place_location_designator=self.place_entity_designator,
arm_designator=self.arm_designator,
area="temp")
self.placeaction1 = PlaceSingleItem(
robot=robot, place_designator=self.place_designator)
self.placeaction2 = PlaceSingleItem(
robot=robot, place_designator=self.place_designator)
with self:
smach.StateMachine.add("MOVE_TO_TABLE1",
states.NavigateToSymbolic(
robot,
{self.table_designator: "in_front_of"},
self.table_designator),
transitions={
'arrived': 'INSPECT_TABLE',
'unreachable': 'MOVE_TO_TABLE2',
'goal_not_defined': 'INSPECT_TABLE'
})
smach.StateMachine.add("MOVE_TO_TABLE2",
states.NavigateToSymbolic(
robot,
{self.table_designator: "in_front_of"},
self.table_designator),
transitions={
'arrived': 'INSPECT_TABLE',
'unreachable': 'INSPECT_TABLE',
'goal_not_defined': 'INSPECT_TABLE'
})
if pdf_writer:
# Designator to store the classificationresults
# The Inspect-state (INSPECT_TABLE) gathers a list of ClassificationResults for Entities on the table
# These are passed to the pdf_writer
class_designator = ds.VariableDesignator(
[],
resolve_type=[
robot_skills.classification_result.ClassificationResult
])
# Add the designator to the pdf writer state
pdf_writer.set_designator(class_designator)
smach.StateMachine.add("INSPECT_TABLE",
states.Inspect(
robot=robot,
entityDes=self.table_designator,
objectIDsDes=class_designator,
searchArea=GRAB_SURFACE,
navigation_area="in_front_of"),
transitions={
"done": "WRITE_PDF",
"failed": "failed"
})
smach.StateMachine.add("WRITE_PDF",
pdf_writer,
transitions={"done": "GRAB_ITEM_1"})
else:
smach.StateMachine.add("INSPECT_TABLE",
states.Inspect(
robot=robot,
entityDes=self.table_designator,
objectIDsDes=None,
searchArea=GRAB_SURFACE,
navigation_area="in_front_of"),
transitions={
"done": "GRAB_ITEM_1",
"failed": "failed"
})
smach.StateMachine.add("GRAB_ITEM_1",
GrabSingleItem(
robot=robot,
grab_designator=grab_designator_1),
transitions={
"succeeded": "GRAB_ITEM_2",
"failed": "GRAB_ITEM_2"
})
smach.StateMachine.add("GRAB_ITEM_2",
GrabSingleItem(
robot=robot,
grab_designator=grab_designator_2),
transitions={
"succeeded": "MOVE_TO_PLACE",
"failed": "MOVE_TO_PLACE"
})
smach.StateMachine.add("MOVE_TO_PLACE",
states.NavigateToSymbolic(
robot, {self.cabinet: "in_front_of"},
self.cabinet),
transitions={
'arrived': 'PLACE_ITEM_1',
'unreachable': 'PLACE_ITEM_1',
'goal_not_defined': 'PLACE_ITEM_1'
})
smach.StateMachine.add("PLACE_ITEM_1",
self.placeaction1,
transitions={
"succeeded": "PLACE_ITEM_2",
"failed": "PLACE_ITEM_2"
})
smach.StateMachine.add("PLACE_ITEM_2",
self.placeaction2,
transitions={
"succeeded": "succeeded",
"failed": "failed"
})
def __init__(self, robot):
""" Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(
self, outcomes=["succeeded", "failed", "aborted"])
# Create designators
trashbin_designator = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id,
name='trashbin_designator')
# Look if there is a second trash bin present
# trashbin_designator2 = None
if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"):
trashbin_designator2 = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator2')
next_state = "HELPER_WAYPOINT"
rospy.loginfo("There is a second trash bin")
else:
rospy.loginfo("There is no second trash bin")
next_state = "ANNOUNCE_END"
# drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id)
helper_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint)
end_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint)
arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
with self:
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
CHALLENGE_KNOWLEDGE.starting_point),
transitions={
"Done": "SAY_START_CHALLENGE",
"Aborted": "SAY_START_CHALLENGE",
"Failed": "SAY_START_CHALLENGE"
})
smach.StateMachine.add("SAY_START_CHALLENGE",
states.Say(
robot,
"I will start cleaning up the trash",
block=True),
transitions={'spoken': "PICK_UP_TRASH"})
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(
robot=robot,
trashbin_designator=trashbin_designator,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH",
"failed": "HELPER_WAYPOINT",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH",
DropDownTrash(robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK",
states.Say(robot,
"First bag has been dropped at the collection zone",
block=False),
transitions={'spoken': next_state})
if next_state == "HELPER_WAYPOINT":
smach.StateMachine.add(
"HELPER_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=helper_waypoint_designator),
transitions={
"arrived": "PICK_UP_TRASH2",
"goal_not_defined": "PICK_UP_TRASH2",
"unreachable": "PICK_UP_TRASH2"
})
smach.StateMachine.add(
"PICK_UP_TRASH2",
PickUpTrash(robot=robot,
trashbin_designator=trashbin_designator2,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH2",
"failed": "ANNOUNCE_END",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH2",
DropDownTrash(
robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK2",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK2",
states.Say(
robot,
"Second bag has been dropped at the collection zone."
"All the thrash has been taken care of",
block=False),
transitions={'spoken': 'ANNOUNCE_END'})
smach.StateMachine.add("ANNOUNCE_END",
states.Say(
robot,
"I have finished taking out the trash.",
block=False),
transitions={'spoken': 'NAVIGATE_OUT'})
smach.StateMachine.add(
"NAVIGATE_OUT",
states.NavigateToWaypoint(
robot=robot, waypoint_designator=end_waypoint_designator),
transitions={
"arrived": "succeeded",
"goal_not_defined": "succeeded",
"unreachable": "succeeded"
})
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"})
return "succeeded"
smach.StateMachine.add("SELECT_ENTITY",
smach.CBState(select_entity),
transitions={"succeeded": "succeeded",
"no_entities": "failed"})
if __name__ == "__main__":
rospy.init_node("test_furniture_inspection")
# Robot
_robot = get_robot_from_argv(index=1)
# Test data
furniture = ds.EdEntityDesignator(robot=_robot, id="desk")
entity_designator = ds.VariableDesignator(resolve_type=Entity)
ps = geometry_msgs.msg.PointStamped()
ps.header.frame_id = "/map"
ps.point.x = 2.0
ps.point.y = -1.0
ps.point.z = 1.0
user_data = smach.UserData()
user_data["laser_dot"] = ps
sm = InspectFurniture(robot=_robot, furniture_designator=furniture, entity_designator=entity_designator.writeable)
sm.execute(user_data)
import robot_smach_states.util.designators as ds
from robot_smach_states import Grab
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Put an imaginary object in the world model and grasp it using the "
"'Grab' smach state")
parser.add_argument("x", type=float, help="x-coordinate (in map) of the imaginary object")
parser.add_argument("y", type=float, help="y-coordinate (in map) of the imaginary object")
parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object")
parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_grasping")
robot = get_robot(args.robot)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
item = ds.EdEntityDesignator(robot, id=entity_id)
arm = ds.UnoccupiedArmDesignator(robot, {})
grab_state = Grab(robot, item, arm)
grab_state.execute()
def __init__(self, robot, manipulated_items):
"""@param manipulated_items is VariableDesignator that will be a list of items manipulated by the robot."""
self.manipulated_items = manipulated_items
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.cabinet = ds.EntityByIdDesignator(robot,
id=CABINET,
name="pick_shelf")
# self.place_shelf = ds.EntityByIdDesignator(robot, id=PLACE_SHELF, name="place_shelf")
not_manipulated = lambda entity: not entity in self.manipulated_items.resolve(
)
def entity_z_pos(entity):
"""
Checks if the entity is between the minimum and maximum grasp height
:param entity:
:return:
"""
if not entity._pose:
return False
return MIN_GRASP_HEIGHT < entity._pose.p.z() < MAX_GRASP_HEIGHT
# select the entity closest in x direction to the robot in base_link frame
def weight_function(entity):
# TODO: return x coordinate of entity.center_point in base_link frame
epose = entity.pose.projectToFrame(
robot.robot_name + "/base_link",
robot.tf_listener) # Get position in base_link
p = epose.frame.p
return p.x()**2
self.current_item = ds.LockingDesignator(ds.EdEntityDesignator(
robot,
criteriafuncs=[
not_ignored, size, not_manipulated, min_entity_height,
entity_z_pos, max_width
],
weight_function=weight_function,
debug=False,
name="item"),
name="current_item")
#This makes that the empty spot is resolved only once, even when the robot moves. This is important because the sort is based on distance between robot and constraint-area
# self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position")
self.place_position = ds.LockingDesignator(EmptyShelfDesignator(
robot, self.cabinet, name="placement", area=PLACE_SHELF),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {
'required_trajectories': ['prepare_grasp'],
'required_goals': ['carrying_pose'],
'required_gripper_types': [arms.GripperTypes.GRASPING],
'required_arm_name': PREFERRED_ARM
},
name="empty_arm_designator")
self.arm_with_item_designator = ds.ArmHoldingEntityDesignator(
robot, {'required_objects': [self.current_item]}, {
"required_trajectories": ["prepare_place"],
"required_goals": ["reset", "handover_to_human"],
'required_gripper_types': [arms.GripperTypes.GRASPING]
},
name="arm_with_item_designator")
# print "{0} = pick_shelf".format(self.pick_shelf)
# print "{0} = current_item".format(self.current_item)
# print "{0} = place_position".format(self.place_position)
# print "{0} = empty_arm_designator".format(self.empty_arm_designator)
# print "{0} = arm_with_item_designator".format(self.arm_with_item_designator)
with self:
# smach.StateMachine.add( "NAV_TO_OBSERVE_PICK_SHELF",
# #states.NavigateToObserve(robot, self.pick_shelf),
# states.NavigateToSymbolic(robot, {self.pick_shelf:"in_front_of", EntityByIdDesignator(robot, id=ROOM):"in"}, self.pick_shelf),
# transitions={ 'arrived' :'LOOKAT_PICK_SHELF',
# 'unreachable' :'LOOKAT_PICK_SHELF',
# 'goal_not_defined' :'LOOKAT_PICK_SHELF'})
smach.StateMachine.add("REMOVE_ENTITIES",
RemoveSegmentedEntities(robot=robot),
transitions={'done': 'LOOKAT_PICK_SHELF'})
smach.StateMachine.add("LOOKAT_PICK_SHELF",
states.LookAtArea(robot,
self.cabinet,
area=PICK_SHELF),
transitions={'succeeded': 'SEGMENT_SHELF'})
smach.StateMachine.add("SEGMENT_SHELF",
SegmentShelf(robot,
entity_id=CABINET,
area_id=PICK_SHELF),
transitions={'done': 'LOCK_ITEM'})
@smach.cb_interface(outcomes=['locked'])
def lock(userdata=None):
self.current_item.lock(
) #This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
if self.current_item.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(
self.current_item.resolve().id))
self.place_position.lock(
) #This determines that self.place_position will lock/cache its result after its resolved the first time.
return 'locked'
smach.StateMachine.add('LOCK_ITEM',
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot,
EntityDescriptionDesignator(
self.current_item,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
Grab(robot, self.current_item,
self.empty_arm_designator),
transitions={
'done': 'STORE_ITEM',
'failed': 'SAY_GRAB_FAILED'
})
smach.StateMachine.add(
"SAY_GRAB_FAILED",
states.Say(robot, ["I couldn't grab this thing"], mood="sad"),
transitions={'spoken': 'UNLOCK_ITEM_AFTER_FAILED_GRAB'}
) # Not sure whether to fail or keep looping with NAV_TO_OBSERVE_PICK_SHELF
@smach.cb_interface(outcomes=['unlocked'])
def unlock_and_ignore(userdata=None):
global ignore_ids
# import ipdb; ipdb.set_trace()
if self.current_item.resolve():
ignore_ids += [self.current_item.resolve().id]
rospy.loginfo("Current_item WAS now locked to {0}".format(
self.current_item.resolve().id))
self.current_item.unlock(
) #This determines that self.current_item can now resolve to a new value on the next call
self.place_position.unlock(
) #This determines that self.place_position can now resolve to a new position on the next call
return 'unlocked'
smach.StateMachine.add('UNLOCK_ITEM_AFTER_FAILED_GRAB',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'failed'})
@smach.cb_interface(outcomes=['stored'])
def store_as_manipulated(userdata=None):
# manipulated_items.current += [self.current_item.current]
item_list = manipulated_items.resolve()
item_list += [self.current_item.resolve()]
w = ds.VariableWriter(manipulated_items)
w.write(item_list)
return 'stored'
smach.StateMachine.add(
'STORE_ITEM',
smach.CBState(store_as_manipulated),
transitions={'stored': 'LOOKAT_PLACE_SHELF'})
smach.StateMachine.add("LOOKAT_PLACE_SHELF",
states.LookAtArea(robot,
self.cabinet,
area=PLACE_SHELF),
transitions={'succeeded': 'PLACE_ITEM'})
smach.StateMachine.add("PLACE_ITEM",
Place(robot, self.current_item,
self.place_position,
self.arm_with_item_designator),
transitions={
'done': 'RESET_HEAD_PLACE',
'failed': 'RESET_HEAD_HUMAN'
})
smach.StateMachine.add(
"RESET_HEAD_PLACE",
states.CancelHead(robot),
transitions={'done': 'UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE'})
smach.StateMachine.add(
"RESET_HEAD_HUMAN",
states.CancelHead(robot),
transitions={'done': 'SAY_HANDOVER_TO_HUMAN'})
smach.StateMachine.add('UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add(
"SAY_HANDOVER_TO_HUMAN",
states.Say(robot, [
"I'm can't get rid of this item myself, can somebody help me maybe?"
]),
transitions={'spoken': 'HANDOVER_TO_HUMAN'})
smach.StateMachine.add('HANDOVER_TO_HUMAN',
states.HandoverToHuman(
robot, self.arm_with_item_designator),
transitions={
'succeeded': 'UNLOCK_AFTER_HANDOVER',
'failed': 'UNLOCK_AFTER_HANDOVER'
})
smach.StateMachine.add('UNLOCK_AFTER_HANDOVER',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': '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",
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",
Grab(
robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals":
["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")),
transitions={
"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"
})
smach.StateMachine.add(
"ARM_RESET",
ArmToJointConfig(
robot,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={"required_goals": ["reset"]},
name="empty_arm_designator"), "reset"),
transitions={
"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"
})
smach.StateMachine.add(
'SAY_GRAB_SUCCESS',
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',
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',
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',
Inspect(robot, store_entity_des),
transitions={
"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"
})
arm_properties_place = {
"required_trajectories": ["prepare_place"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
}
arm_designator_place = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_IN_TRASH',
Place(robot, selected_entity_designator,
trash_place_pose,
arm_designator_place),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
arm_designator_place_store = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_TO_STORE',
Place(robot, selected_entity_designator,
store_entity_des,
arm_designator_place_store,
"on_top_of"),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
smach.StateMachine.add(
'SAY_PLACE_SUCCESS',
Say(robot, [
"Bye bye!", "Yeah!", "Successfully disposed the item",
"Another score for {}".format(robot.robot_name)
],
block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add(
'SAY_PLACE_FAILED',
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"})
def single_item(robot,
results_writer,
cls,
support,
waypoint,
inspect_from_area=None,
non_strict_class=False,
search_area='on_top_of'):
"""
Benchmark grasping for a single item. Outputs a record dictionary
:param robot: an instance of Robot
:param results_writer: a csv.DictWriter to which the output record is written
:param cls: class/type of item to grab
:param support: ID of the entity supporting the item-to-grab
:param waypoint: From where should the robot start the grasp
:param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection
:param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object
:param search_area: which area of the support-entity to search/inspect for an item of the given class
:return: a dict with the benchmark result
"""
grasp_cls = ds.Designator(cls, name='grasp_cls')
support_entity = ds.EdEntityDesignator(robot,
id=support,
name='support_entity')
entity_ids = ds.VariableDesignator([],
resolve_type=[ClassificationResult],
name='entity_ids')
waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint')
arm = ds.LockingDesignator(
ds.UnoccupiedArmDesignator(robot,
name='unoccupied-arm',
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
}))
arm.lock()
record = {
'robot': robot.robot_name,
'start_waypoint': waypoint,
'expected_class': cls,
'id': None,
'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
}
try:
say_announce = Say(
robot,
sentence="Please put a {cls} {search_area} the {support}".format(
cls=cls, support=support, search_area=search_area))
say_announce.execute()
nav_to_start = NavigateToWaypoint(robot,
waypoint_designator=waypoint_des,
look_at_designator=support_entity)
assert nav_to_start.execute(
) == 'arrived', "I did not arrive at the start"
inspect = Inspect(robot=robot,
entityDes=support_entity,
objectIDsDes=entity_ids,
navigation_area=inspect_from_area,
searchArea=search_area)
record['inspect_start'] = time.time()
assert inspect.execute(
) == 'done', "I could not inspect the support entity"
record['inspect_end'] = time.time()
record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] -
record['inspect_start'])
inspection_result = entity_ids.resolve(
) # type: List[ClassificationResult]
if inspection_result:
if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class:
rospy.loginfo("Any item will match")
matching_results = inspection_result
else:
matching_results = [
result for result in inspection_result
if result.type == grasp_cls.resolve()
]
rospy.loginfo("Found {} items of class {}".format(
len(matching_results), grasp_cls.resolve()))
if matching_results:
if len(matching_results) > 1:
rospy.logwarn(
"There are multiple items OK to grab, will select the last one"
)
record['observed_class'] = ' '.join(
[result.type for result in matching_results])
else:
record['observed_class'] = matching_results[-1].type
selected_entity_id = matching_results[-1].id
rospy.loginfo("Selected entity {} for grasping".format(
selected_entity_id))
grasp_entity = ds.EdEntityDesignator(robot,
id=selected_entity_id)
record['id'] = selected_entity_id[:6]
entity = grasp_entity.resolve() # type: Entity
if entity:
vector_stamped = entity.pose.extractVectorStamped(
) # type: VectorStamped
record['x'] = '{:.3f}'.format(vector_stamped.vector.x())
record['y'] = '{:.3f}'.format(vector_stamped.vector.y())
record['z'] = '{:.3f}'.format(vector_stamped.vector.z())
grab_state = Grab(robot, grasp_entity, arm)
record['grab_start'] = time.time()
assert grab_state.execute() == 'done', "I couldn't grasp"
record['grab_end'] = time.time()
record['grab_duration'] = '{:3.3f}'.format(
record['grab_end'] - record['grab_start'])
assert nav_to_start.execute(
) == 'arrived', "I could not navigate back to the start"
rospy.logwarn("Robot will turn around to drop the {}".format(
grasp_cls.resolve()))
force_drive = ForceDrive(robot, 0, 0, 1,
3.14) # rotate 180 degs in pi seconds
force_drive.execute()
say_drop = Say(
robot,
sentence="I'm going to drop the item, please hold it!")
say_drop.execute()
drop_it = SetGripper(robot,
arm_designator=arm,
grab_entity_designator=grasp_entity)
drop_it.execute()
force_drive_back = ForceDrive(
robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds
force_drive_back.execute()
nav_to_start.execute()
else:
raise AssertionError("No {} found".format(grasp_cls.resolve()))
else:
rospy.logerr("No entities found at all :-(")
except AssertionError as assertion_err:
say_fail = Say(robot, sentence=assertion_err.message + ", sorry")
say_fail.execute()
finally:
results_writer.writerow(record)
return record
