self._robot.head.wait_for_motion_done()
return 'succeeded'
if __name__ == "__main__":
from robot_skills import get_robot
from ..util.designators import EdEntityDesignator, UnoccupiedArmDesignator
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=UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=EdEntityDesignator(
robot, id=point_at, name='point_at_des'),
look_at_designator=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)
import rospy
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,
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] != '_'])
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
robot = get_robot(args.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()
import sys
from robot_skills.util import kdl_conversions
from robot_skills import get_robot
from robot_smach_states.navigation.guidance import _detect_operator_behind_robot
robot = get_robot(sys.argv[1])
a = kdl_conversions.VectorStamped(-1, 0, 1,
'/{}/base_link'.format(sys.argv[1]))
hero.head.look_at_point(a)
_detect_operator_behind_robot(robot)
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()
