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 main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_gpsr')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
while True:
# Navigate to the GPSR meeting point
if not skip and not finished:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
while True:
while True and not test:
try:
robot.hmi.query(description="",
grammar="T -> %s" % robot_name,
target="T")
except hmi.TimeoutException:
continue
else:
break
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
continue
if not test:
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no',
'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) #be sure lights are blue
robot.head.look_at_standing_person()
robot.leftArm.reset()
robot.leftArm.send_gripper_goal('close', 0.0)
robot.rightArm.reset()
robot.rightArm.send_gripper_goal('close', 0.0)
robot.torso.reset()
if task_result.succeeded:
# Keep track of the number of performed tasks
no_of_tasks_performed += 1
if no_of_tasks_performed >= no_of_tasks:
finished = True
# If we succeeded, we can say something optimistic after reporting to the operator
if no_of_tasks_performed == 1:
task_word = "task"
else:
task_word = "tasks"
report += " I performed {} {} so far, still going strong!".format(
no_of_tasks_performed, task_word)
if rospy.get_time() - start_time > 60 * 15:
finished = True
if finished and not skip:
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.exit_waypoint),
radius=0.3)
nwc.execute()
robot.speech.speak("Thank you very much, and goodbye!", block=True)
break
def main():
rospy.init_node("demo")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
rospy.loginfo("[DEMO] Parameters:")
rospy.loginfo("[DEMO] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[DEMO] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[DEMO] running a restart")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
elif robot_name == 'hero':
from robot_skills.hero import Hero as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_demo')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger")
while True:
# Navigate to the GPSR meeting point
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
timeout_count = 0
while True:
rospy.loginfo("Waiting for trigger")
trigger.execute()
base_loc = robot.base.get_location()
base_pose = base_loc.frame
print base_pose
location_id = "starting_point"
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
robot.head.look_at_standing_person()
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
timeout_count = 0
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
if timeout_count >= 3:
robot.hmi.restart_dragonfly()
timeout_count = 0
rospy.logwarn("[GPSR] Dragonfly restart")
else:
timeout_count += 1
rospy.logwarn(
"[GPSR] Timeout_count: {}".format(timeout_count))
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) # be sure lights are blue
robot.head.look_at_standing_person()
for arm in robot.arms.itervalues():
arm.reset()
arm.send_gripper_goal('close', 0.0)
robot.torso.reset()
rospy.loginfo("Driving back to the starting point")
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot, id=location_id),
radius=0.3)
nwc.execute()
def setup_statemachine(robot):
item = VariableDesignator(resolve_type=Entity)
arm = Designator(robot.leftArm)
sm = StateMachine(outcomes=['done'])
with sm:
# Start challenge via StartChallengeRobust
StateMachine.add('START_CHALLENGE_ROBUST',
StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
transitions={'Done': 'OPEN_DISHWASHER',
'Aborted': 'done',
'Failed': 'OPEN_DISHWASHER'})
StateMachine.add('OPEN_DISHWASHER', NavigateAndOpenDishwasher(robot),
transitions={'succeeded': 'GRAB',
'failed': 'GRAB'})
# StateMachine.add('FIND_OBJECT', FindObject(robot, item),
# transitions={'found': 'GRAB',
# 'not_found': 'SAY_EXIT'})
#
# StateMachine.add('GRAB', Grab(robot, item, arm),
# transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER',
# 'failed': 'FIND_OBJECT2'})
#
# StateMachine.add('FIND_OBJECT2', FindObject(robot, item),
# transitions={'found': 'GRAB2',
# 'not_found': 'SAY_EXIT'})
#
# StateMachine.add('GRAB2', Grab(robot, item, arm),
# transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER',
# 'failed': 'SAY_EXIT'})
StateMachine.add('GRAB', GrabRobust(robot),
transitions={'succeeded': 'PLACE_DISHWASHER',
'failed': 'SAY_EXIT'})
StateMachine.add('PLACE_DISHWASHER', NavigateAndPlaceDishwasher(robot),
transitions={'succeeded': 'SAY_EXIT',
'failed': 'SAY_EXIT'})
StateMachine.add('SAY_EXIT', Say(robot, ["I will move to the exit now. See you guys later!"], block=False),
transitions={'spoken': 'GO_TO_EXIT'})
StateMachine.add('GO_TO_EXIT',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_1), radius=0.7),
transitions={'arrived': 'done',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'})
StateMachine.add('GO_TO_EXIT_2',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_2), radius=0.5),
transitions={'arrived': 'done',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'})
StateMachine.add('GO_TO_EXIT_3',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_3), radius=0.5),
transitions={'arrived': 'done',
'unreachable': 'done',
'goal_not_defined': 'done'})
return sm
def main():
rospy.init_node("demo")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
rospy.loginfo("[DEMO] Parameters:")
rospy.loginfo("[DEMO] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[DEMO] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[DEMO] running a restart")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
elif robot_name == 'hero':
from robot_skills.hero import Hero as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_demo')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!", block=False)
if restart:
robot.speech.speak("Performing a restart. So sorry about that last time!", block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger")
while True:
# Navigate to the GPSR meeting point
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
timeout_count = 0
while True:
rospy.loginfo("Waiting for trigger")
trigger.execute()
base_loc = robot.base.get_location()
base_pose = base_loc.frame
print base_pose
location_id = "starting_point"
robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped(base_pose, "/map"),
type="waypoint")
robot.head.look_at_standing_person()
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
timeout_count = 0
break
except hmi.TimeoutException:
robot.speech.speak(random.sample(knowledge.not_understood_sentences, 1)[0])
if timeout_count >= 3:
robot.hmi.restart_dragonfly()
timeout_count = 0
rospy.logwarn("[GPSR] Dragonfly restart")
else:
timeout_count += 1
rospy.logwarn("[GPSR] Timeout_count: {}".format(timeout_count))
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) # be sure lights are blue
robot.head.look_at_standing_person()
for arm in robot.arms.itervalues():
arm.reset()
arm.send_gripper_goal('close', 0.0)
robot.torso.reset()
rospy.loginfo("Driving back to the starting point")
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=location_id),
radius=0.3)
nwc.execute()
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
state_machine.userdata['item_picked'] = None
with state_machine:
# Intro
# StateMachine.add('START_CHALLENGE_ROBUST',
# Initialize(robot),
# transitions={'initialized': 'SAY_START', 'abort': 'done'})
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"), # ToDo: in knowledge
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
StateMachine.add('SAY_START',
Say(robot,
"Let's set the table baby! If there are any chairs near the kitchen_table, please remove them",
block=False),
transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'})
# The pre-work
StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD',
NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"),
transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
'failed': 'SAY_OPEN_FAILED'})
StateMachine.add('SAY_OPEN_FAILED',
Say(robot, "I failed to open the cupboard drawer"),
transitions={'spoken': 'WAIT_OPEN'})
StateMachine.add('WAIT_OPEN',
WaitTime(robot, 5),
transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'})
StateMachine.add('SAY_OPEN_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# The loop
StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of",
required_items),
transitions={'succeeded': 'PLACE_ITEM_ON_TABLE',
'failed': 'CHECK_IF_WE_HAVE_IT_ALL'})
StateMachine.add('PLACE_ITEM_ON_TABLE',
NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"),
transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL',
'failed': 'WAIT'})
StateMachine.add('WAIT',
WaitTime(robot, 2),
transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'})
StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL',
CBState(check_if_we_have_it_all, cb_args=[robot]),
transitions={'we_have_it_all': 'SAY_END_CHALLENGE',
'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# Outro
StateMachine.add('SAY_END_CHALLENGE',
Say(robot, "That was all folks, have a nice meal!"),
transitions={'spoken': 'done'})
StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD',
NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"),
transitions={'succeeded': 'done',
'failed': 'done'})
return state_machine
def setup_statemachine(robot):
furniture = EntityByIdDesignator(robot, 'selected_furniture')
# arm_designator = ArmDesignator(robot.arms, robot.arms['left'])
arm_designator = UnoccupiedArmDesignator(robot.arms, robot.arms['left'])
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add("START_CHALLENGE_ROBUST",
StartChallengeRobust(robot, challenge_knowledge.initial_pose),
transitions={"Done": "NAVIGATE_TO_SSL_WAYPOINT",
"Failed": "Aborted",
"Aborted": "Aborted"})
# smach.StateMachine.add('SAY_STARTING_OPEN_CHALLENGE',
# robot_smach_states.Say(robot, ["Hi there, welcome to the open challenge!"], block=False),
# transitions={"spoken": "NAVIGATE_TO_SSL_WAYPOINT"})
smach.StateMachine.add("NAVIGATE_TO_SSL_WAYPOINT",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.ssl_waypoint),
radius=0.3),
transitions={'arrived': 'SSL_DEMO',
'unreachable': 'SSL_DEMO',
'goal_not_defined': 'SSL_DEMO'})
smach.StateMachine.add("SSL_DEMO",
SSLDemo(robot),
transitions={"done": "NAVIGATE_TO_LASER_DEMO", 'preempted': 'Aborted'})
smach.StateMachine.add("NAVIGATE_TO_LASER_DEMO",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.raytrace_waypoint),
radius=0.3),
transitions={'arrived': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO',
'unreachable': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO',
'goal_not_defined': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO'})
smach.StateMachine.add("RESET_HEAD_BEFORE_RAYTRACE_DEMO",
ResetHead(robot),
transitions={'done': 'SAY_PEOPLE_DETECTOR'})
smach.StateMachine.add("SAY_PEOPLE_DETECTOR",
Say(robot, "Now I will show you my awesome people detector"),
transitions={"spoken": "WAIT_FOR_TRIGGER_BEFORE_RAYTRACE_DEMO"})
smach.StateMachine.add("WAIT_FOR_TRIGGER_BEFORE_RAYTRACE_DEMO",
WaitForTrigger(robot, ["continue"], "/amigo/trigger"),
transitions={'continue': 'RAYTRACE_DEMO',
'preempted': 'RAYTRACE_DEMO'})
smach.StateMachine.add("RAYTRACE_DEMO",
RayTraceDemo(robot, breakout_id=challenge_knowledge.raytrace_waypoint),
transitions={"done": "SAY_RAYTRACE_SELECTOR"})
smach.StateMachine.add("SAY_RAYTRACE_SELECTOR",
Say(robot, "You can interact with me by pointing at objects!"),
transitions={"spoken": "RAYTRACE_SELECTOR"})
smach.StateMachine.add("RAYTRACE_SELECTOR",
SimpleRayTraceSelector(robot, waypoint=None, furniture_designator=furniture),
transitions={
"waypoint": 'NAVIGATE_TO_WAYPOINT',
"furniture": 'NAVIGATE_TO_FURNITURE',
"grasp": 'INSPECT_AND_GRAB',
"done": 'RAYTRACE_SELECTOR'
})
smach.StateMachine.add("NAVIGATE_TO_FURNITURE",
NavigateToSymbolic(robot,
entity_designator_area_name_map={furniture: 'in_front_of'},
entity_lookat_designator=furniture),
transitions={
'arrived': 'NAVIGATE_BACK_TO_LASER_DEMO',
'unreachable': 'NAVIGATE_BACK_TO_LASER_DEMO',
'goal_not_defined': 'NAVIGATE_BACK_TO_LASER_DEMO',
})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id='final_waypoint'),
radius=0.3),
transitions={'arrived': 'NAVIGATE_BACK_TO_LASER_DEMO',
'unreachable': 'NAVIGATE_BACK_TO_LASER_DEMO',
'goal_not_defined': 'NAVIGATE_BACK_TO_LASER_DEMO'})
smach.StateMachine.add("INSPECT_AND_GRAB",
InspectAndGrab(robot, supporting_entity_designator=furniture,
arm_designator=arm_designator),
transitions={
'succeeded': 'NAVIGATE_BACK_TO_LASER_DEMO',
'inspect_failed': 'NAVIGATE_BACK_TO_LASER_DEMO',
'grasp_failed': 'NAVIGATE_BACK_TO_LASER_DEMO'
})
smach.StateMachine.add("HANDOVER_TO_HUMAN", HandoverToHuman(robot, arm_designator, timeout=10),
transitions={
'succeeded': 'NAVIGATE_BACK_TO_LASER_DEMO',
'failed': 'NAVIGATE_BACK_TO_LASER_DEMO',
})
smach.StateMachine.add("NAVIGATE_BACK_TO_LASER_DEMO",
NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.raytrace_waypoint),
radius=0.3),
transitions={'arrived': 'RAYTRACE_SELECTOR',
'unreachable': 'RAYTRACE_SELECTOR',
'goal_not_defined': 'RAYTRACE_SELECTOR'})
# smach.StateMachine.add("NAVIGATE_TO_ORDER_COUNTER",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(
# robot=robot,
# id=challenge_knowledge.order_counter_waypoint),
# radius=0.3),
# transitions={'arrived': 'ORDER_COUNTER',
# 'unreachable': 'ORDER_COUNTER',
# 'goal_not_defined': 'ORDER_COUNTER'})
#
# smach.StateMachine.add("ORDER_COUNTER",
# OrderCounter(robot, room_id=challenge_knowledge.audience_room,
# beercounter=beercounter),
# transitions={"done": "NAVIGATE_TO_HSR_DEMO"})
#
# smach.StateMachine.add("NAVIGATE_TO_HSR_DEMO",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(
# robot=robot,
# id=challenge_knowledge.hsr_demo_waypoint),
# radius=0.025),
# transitions={'arrived': 'WAIT_FOR_BEER',
# 'unreachable': 'CANNOT_REACH_BEER_LOCATION',
# 'goal_not_defined': 'CANNOT_REACH_BEER_LOCATION'})
#
# smach.StateMachine.add("CANNOT_REACH_BEER_LOCATION",
# robot_smach_states.Say(robot, "I cannot reach my beer buddy, let's give it another try"),
# transitions={"spoken": "Done"})
#
# smach.StateMachine.add("WAIT_FOR_BEER",
# HsrInteraction(robot=robot, beercounter=beercounter),
# transitions={"done": "RETURN_TO_AUDIENCE"})
#
# smach.StateMachine.add("RETURN_TO_AUDIENCE",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(
# robot=robot,
# id=challenge_knowledge.order_counter_waypoint),
# radius=0.3),
# transitions={'arrived': 'SAY_BEER',
# 'unreachable': 'SAY_BEER',
# 'goal_not_defined': 'SAY_BEER'})
#
# smach.StateMachine.add("SAY_BEER",
# robot_smach_states.Say(robot, "Hey guys, here's your beer. That will be all. "
# "By the way, if you leave the balcony door open,"
# "birds will fly in"),
# transitions={"spoken": "Done"})
return sm
def main():
rospy.init_node("ee_gpsr")
parser = argparse.ArgumentParser()
parser.add_argument('robot', help='Robot name')
parser.add_argument('--once',
action='store_true',
help='Turn on infinite loop')
parser.add_argument('--skip', action='store_true', help='Skip enter/exit')
parser.add_argument('sentence', nargs='*', help='Optional sentence')
args = parser.parse_args()
rospy.loginfo('args: %s', args)
mock_sentence = " ".join(
[word for word in args.sentence if word[0] != '_'])
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if args.robot == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif args.robot == 'sergio':
from robot_skills.sergio import Sergio as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
# Sleep for 1 second to make sure everything is connected
time.sleep(1)
command_center = CommandCenter(robot)
challenge_knowledge = load_knowledge('challenge_eegpsr')
command_center.set_grammar(
os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not args.skip:
# Wait for door, enter arena
s = StartChallengeRobust(robot, challenge_knowledge.initial_pose)
s.execute()
# Move to the start location
nwc = NavigateToWaypoint(robot,
EntityByIdDesignator(
robot,
id=challenge_knowledge.starting_pose),
radius=0.3)
nwc.execute()
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
sentence = " ".join([word for word in args.sentence if word[0] != '_'])
first = True
if sentence:
semantics = command_center.parse_command(sentence)
if not semantics:
rospy.logerr("Cannot parse \"{}\"".format(sentence))
return
command_center.execute_command(semantics)
else:
while True:
if first:
# First sentence robot says
sentences = [
"Hello there! Welcome to the double E GPSR. You can give me an order, but wait for the ping."
]
else:
# Sentence robot says after completing a task
sentences = [
"Hello there, you look lovely! I'm here to take a new order, but wait for the ping!"
]
# These sentences are for when the first try fails
# (Robot says "Do you want me to ...?", you say "No", then robot says sentence below)
sentences += [
"I'm so sorry! Can you please speak louder and slower? And wait for the ping!",
"Again, I am deeply sorry. Bad robot! Please try again, but wait for the ping!",
"You and I have communication issues. Speak up! Tell me what you want, but wait for the ping"
]
hey_robot_wait_forever(robot)
res = command_center.request_command(ask_confirmation=True,
ask_missing_info=False,
timeout=600,
sentences=sentences)
if not res:
continue
first = False
(sentence, semantics) = res
if not command_center.execute_command(semantics):
robot.speech.speak("I am truly sorry, let's try this again")
if args.once:
break
if not args.skip:
nwc = NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.starting_pose),
radius=0.3)
nwc.execute()
def main():
rospy.init_node("ee_gpsr")
parser = argparse.ArgumentParser()
parser.add_argument('robot', help='Robot name')
parser.add_argument('--once', action='store_true', help='Turn on infinite loop')
parser.add_argument('--skip', action='store_true', help='Skip enter/exit')
parser.add_argument('sentence', nargs='*', help='Optional sentence')
args = parser.parse_args()
rospy.loginfo('args: %s', args)
mock_sentence = " ".join([word for word in args.sentence if word[0] != '_'])
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if args.robot == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif args.robot == 'sergio':
from robot_skills.sergio import Sergio as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
# Sleep for 1 second to make sure everything is connected
time.sleep(1)
command_center = CommandCenter(robot)
challenge_knowledge = load_knowledge('challenge_eegpsr')
command_center.set_grammar(os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not args.skip:
# Wait for door, enter arena
s = StartChallengeRobust(robot, challenge_knowledge.initial_pose)
s.execute()
# Move to the start location
nwc = NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius = 0.3)
nwc.execute()
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
sentence = " ".join([word for word in args.sentence if word[0] != '_'])
first = True
if sentence:
semantics = command_center.parse_command(sentence)
if not semantics:
rospy.logerr("Cannot parse \"{}\"".format(sentence))
return
command_center.execute_command(semantics)
else:
while True:
if first:
# First sentence robot says
sentences = ["Hello there! Welcome to the double E GPSR. You can give me an order, but wait for the ping."]
else:
# Sentence robot says after completing a task
sentences = ["Hello there, you look lovely! I'm here to take a new order, but wait for the ping!"]
# These sentences are for when the first try fails
# (Robot says "Do you want me to ...?", you say "No", then robot says sentence below)
sentences += [
"I'm so sorry! Can you please speak louder and slower? And wait for the ping!",
"Again, I am deeply sorry. Bad robot! Please try again, but wait for the ping!",
"You and I have communication issues. Speak up! Tell me what you want, but wait for the ping"
]
hey_robot_wait_forever(robot)
res = command_center.request_command(ask_confirmation=True, ask_missing_info=False, timeout=600, sentences=sentences)
if not res:
continue
first = False
(sentence, semantics) = res
if not command_center.execute_command(semantics):
robot.speech.speak("I am truly sorry, let's try this again")
if args.once:
break
if not args.skip:
nwc = NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius = 0.3)
nwc.execute()
def main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
eegpsr = rospy.get_param('~eegpsr', False)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
if time_limit == 0:
time_limit = 999
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
rospy.loginfo("[GPSR] time_limit = {}".format(time_limit))
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
elif robot_name == 'hero':
from robot_skills.hero import Hero as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
if eegpsr:
knowledge = load_knowledge('challenge_eegpsr')
else:
knowledge = load_knowledge('challenge_gpsr')
conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar, knowledge.grammar_target, knowledge)
conversation_engine.test = test
conversation_engine.skip = skip
conversation_engine.tasks_to_be_done = no_of_tasks
conversation_engine.time_limit = time_limit
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!", block=False)
if restart:
robot.speech.speak("Performing a restart. So sorry about that last time!", block=False)
conversation_engine._start_wait_for_command(knowledge.grammar, knowledge.grammar_target)
rospy.spin()
def main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
eegpsr = rospy.get_param('~eegpsr', False)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
if time_limit == 0:
time_limit = 999
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
rospy.loginfo("[GPSR] time_limit = {}".format(time_limit))
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
robot = get_robot(robot_name)
if eegpsr:
knowledge = load_knowledge('challenge_eegpsr')
else:
knowledge = load_knowledge('challenge_gpsr')
conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar,
knowledge.grammar_target,
knowledge)
conversation_engine.test = test
conversation_engine.skip = skip
conversation_engine.tasks_to_be_done = no_of_tasks
conversation_engine.time_limit = time_limit
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
conversation_engine._start_wait_for_command(knowledge.grammar,
knowledge.grammar_target)
rospy.spin()