def task_finished(self, message):
# Navigate to the GPSR meeting point
self.finished = rospy.get_time() - self.start_time > (60 * self.time_limit - 45) and self.tasks_done >= 1
if not self.skip and not self.finished:
self.robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=self.robot,
waypoint_designator=EntityByIdDesignator(robot=self.robot,
id=self.knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
self.robot.head.look_at_standing_person()
self._say_to_user(message)
self._say_to_user("I performed {} {} so far, still going strong!".format(self.tasks_done, "tasks" if self.tasks_done == 1 else "tasks"))
self.timeout_count = 0
self._start_wait_for_command(self.knowledge.grammar, self.knowledge.grammar_target)
else:
rospy.logwarn("Not going to meeting point, challenge has param skip:=true set")
if self.tasks_done >= self.tasks_to_be_done and not self.skip:
nwc = NavigateToWaypoint(robot=self.robot,
waypoint_designator=EntityByIdDesignator(robot=self.robot,
id=self.knowledge.exit_waypoint),
radius=0.3)
self.robot.speech.speak("I'm done now. Thank you very much, and goodbye!", block=True)
nwc.execute()
return
def task_finished(self, message):
# Navigate to the GPSR meeting point
self.finished = rospy.get_time() - self.start_time > (
60 * self.time_limit - 45) and self.tasks_done >= 1
if not self.skip and not self.finished:
self.robot.speech.speak("Moving to the meeting point.",
block=False)
nwc = NavigateToWaypoint(robot=self.robot,
waypoint_designator=EntityByIdDesignator(
robot=self.robot,
id=self.knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
self.robot.head.look_at_standing_person()
self._say_to_user(message)
self._say_to_user(
"I performed {} {} so far, still going strong!".format(
self.tasks_done,
"tasks" if self.tasks_done == 1 else "tasks"))
self.timeout_count = 0
self._start_wait_for_command(self.knowledge.grammar,
self.knowledge.grammar_target)
else:
rospy.logwarn(
"Not going to meeting point, challenge has param skip:=true set"
)
if (self.tasks_done >= self.tasks_to_be_done
or self.finished) and not self.skip:
nwc = NavigateToWaypoint(robot=self.robot,
waypoint_designator=EntityByIdDesignator(
robot=self.robot,
id=self.knowledge.exit_waypoint),
radius=0.3)
self.robot.speech.speak(
"I'm done now. Thank you very much, and goodbye!", block=True)
nwc.execute()
return
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
furniture_designator = ds.VariableDesignator(resolve_type=Entity)
entity_designator = ds.VariableDesignator(resolve_type=Entity)
arm_designator = ds.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 __init__(self, robot):
"""
Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
self.spec_des = ds.Designator(knowledge.location_grammar)
self.answer_des = ds.VariableDesignator(resolve_type=HMIResult)
self.entity_des = EntityFromHmiResults(robot, self.answer_des)
self._location_hmi_attempt = 0
self._max_hmi_attempts = 3 # ToDo: parameterize?
@smach.cb_interface(outcomes=["reset"])
def _reset_location_hmi_attempt(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
self._location_hmi_attempt = 0
return "reset"
smach.StateMachine.add("RESET_HMI_ATTEMPT",
smach.CBState(_reset_location_hmi_attempt),
transitions={"reset": "ANNOUNCE_ITEM"})
if WAIT_MODE == WaitMode.SPEECH:
smach.StateMachine.add(
"ANNOUNCE_ITEM",
Say(robot,
"Hello, my name is {}. Please call me by my name. "
"Talk loudly into my microphone and wait for the ping".
format(robot.robot_name),
block=True),
transitions={"spoken": "WAIT_TO_BE_CALLED"})
smach.StateMachine.add(
"WAIT_TO_BE_CALLED",
HearOptions(robot, ["{}".format(robot.robot_name)],
timeout=10),
transitions={
"{}".format(robot.robot_name): "INSTRUCT",
"no_result": "ANNOUNCE_ITEM"
})
elif WAIT_MODE == WaitMode.VISUAL:
smach.StateMachine.add(
"ANNOUNCE_ITEM",
Say(robot,
"Hello, my name is {}. Please step in front of me.".
format(robot.robot_name),
block=True),
transitions={"spoken": "WAIT_TO_BE_CALLED"})
smach.StateMachine.add("WAIT_TO_BE_CALLED",
WaitForPersonInFront(
robot,
attempts=10,
sleep_interval=1.0),
transitions={
"success": "INSTRUCT",
"failed": "SAY_NOT_DETECTED"
})
smach.StateMachine.add(
"SAY_NOT_DETECTED",
Say(robot,
"I did not see you but will try to continue anyway.".
format(robot.robot_name),
block=True),
transitions={"spoken": "INSTRUCT"})
smach.StateMachine.add(
"INSTRUCT",
Say(robot, [
"Please tell me where you would like to go. "
"Talk loudly into my microphone and wait for the ping"
],
block=True),
transitions={"spoken": "LISTEN_FOR_LOCATION"})
smach.StateMachine.add("LISTEN_FOR_LOCATION",
HearOptionsExtra(robot, self.spec_des,
self.answer_des.writeable,
6),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "HANDLE_FAILED_HMI"
})
smach.StateMachine.add(
"ASK_CONFIRMATION",
Say(robot, [
"I hear that you would like to go to the {place}, is this correct?"
],
place=ds.AttrDesignator(self.entity_des,
"id",
resolve_type=str)),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
HearOptions(robot=robot,
options=["yes", "no"]),
transitions={
"yes": "INSTRUCT_FOR_WAIT",
"no": "INSTRUCT",
"no_result": "HANDLE_FAILED_HMI"
})
@smach.cb_interface(outcomes=["retry", "fallback", "failed"])
def _handle_failed_hmi(userdata=None):
""" Handle failed HMI queries so we can try up to x times """
self._location_hmi_attempt += 1 # Increment
if self._location_hmi_attempt == self._max_hmi_attempts:
rospy.logwarn(
"HMI failed for the {} time, returning 'failed'".
format(self._max_hmi_attempts))
if not BACKUP_SCENARIOS:
rospy.logwarn(
"No fallback scenario's available anymore")
return "failed"
backup = BACKUP_SCENARIOS.pop(0)
robot.speech.speak("I am sorry but I did not hear you",
mood="sad",
block=False)
robot.speech.speak(backup.sentence, block=False)
self.answer_des.writeable.write(
HMIResult("", backup.entity_id))
return "fallback"
rospy.loginfo(
"HMI failed for the {} time out of {}, retrying".format(
self._location_hmi_attempt, self._max_hmi_attempts))
return "retry"
smach.StateMachine.add("HANDLE_FAILED_HMI",
smach.CBState(_handle_failed_hmi),
transitions={
"retry": "INSTRUCT",
"fallback": "INSTRUCT_FOR_WAIT",
"failed": "failed"
})
smach.StateMachine.add(
"INSTRUCT_FOR_WAIT",
Say(robot, [
"Let me think how to get to the {entity_id}",
"I will now determine the best route to the {entity_id}"
],
entity_id=ds.AttrDesignator(self.entity_des,
"id",
resolve_type=str)),
transitions={"spoken": "GIVE_DIRECTIONS"})
smach.StateMachine.add("GIVE_DIRECTIONS",
GiveDirections(robot, self.entity_des),
transitions={
"succeeded": "INSTRUCT_FOLLOW",
"failed": "failed"
})
smach.StateMachine.add("INSTRUCT_FOLLOW",
Say(robot, ["Please follow me"],
block=True),
transitions={"spoken": "GUIDE_OPERATOR"})
smach.StateMachine.add("GUIDE_OPERATOR",
GuideToRoomOrObject(robot, self.entity_des),
transitions={
"arrived": "SUCCESS",
"unreachable": "SAY_CANNOT_REACH",
"goal_not_defined": "SAY_CANNOT_REACH",
"lost_operator": "SAY_LOST_OPERATOR",
"preempted": "failed"
})
smach.StateMachine.add(
"SUCCESS",
Say(robot, ["We have arrived"], block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add(
"SAY_CANNOT_REACH",
Say(robot, ["I am sorry but I cannot reach the destination."],
block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add(
"SAY_LOST_OPERATOR",
Say(robot, ["Oops I have lost you completely."], block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add("RETURN_TO_INFORMATION_POINT",
NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, INFORMATION_POINT_ID)),
transitions={
"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "failed"
})
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 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()
robot = Robot()
if len(sys.argv) > 2:
room = sys.argv[2]
else:
room = "kitchen"
rospy.sleep(2) # wait for tf cache to be filled
#nav_state = NavigateToPose(robot, 1, 0, 0)
#nav_state.execute()
#nav_state = NavigateToObserve(robot, ds.EntityByIdDesignator(robot=robot, id="dinnertable"))
#nav_state.execute()
# nav_state = NavigateToSymbolic(robot, {
# ds.EntityByIdDesignator(robot, id=room):"in"},
# ds.EntityByIdDesignator(robot, id=room))
# nav_state.execute()
#nav_state = NavigateToSymbolic(robot, {
# ds.EntityByIdDesignator(robot, id="couch"):"in_front_of"},
# ds.EntityByIdDesignator(robot, id="couch"))
#nav_state.execute()
nav_state = NavigateToWaypoint(robot=robot,
waypoint_designator=ds.EntityByIdDesignator(
robot=robot, id="hsr_demo_waypoint"),
radius=0.025)
nav_state.execute()
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
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()
# ROS
import rospy
# TU/e Robotics
from robot_skills import get_robot_from_argv
# Robot smach states
from robot_smach_states.navigation import NavigateRobust, NavigateToWaypoint
import robot_smach_states.util.designators as ds
if __name__ == "__main__":
# Initialize rosnode
rospy.init_node("test_robust_navigation")
# Construct robot object
robot = get_robot_from_argv(index=1)
# Construct statemachine
sm = NavigateRobust(
robot,
options=[
NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id="foo")),
NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id="bar")),
NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id="initial_pose"))
],
wait_time=1.0)
sm.execute()
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
StartChallengeRobust(robot, STARTING_POINT),
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',
NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_1),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted'])
with sm:
smach.StateMachine.add('START_CHALLENGE_ROBUST',
StartChallengeRobust(robot, STARTING_POINT),
transitions={
'Done': 'GO_TO_SEARCH_POSE',
'Aborted': 'aborted',
'Failed': 'GO_TO_SEARCH_POSE'
})
smach.StateMachine.add('SAY_START',
Say(robot,
"Finding your mates, here we go!",
block=False),
transitions={'spoken': 'GO_TO_SEARCH_POSE'})
smach.StateMachine.add('GO_TO_SEARCH_POSE',
NavigateToWaypoint(robot,
ds.EntityByIdDesignator(
robot, id=SEARCH_POINT),
radius=0.375),
transitions={
'arrived': 'RISE_FOR_THE_PEOPLE',
'goal_not_defined': 'failed',
'unreachable': 'WAIT_SEARCH_POSE'
})
smach.StateMachine.add('WAIT_SEARCH_POSE',
WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_TO_SEARCH_POSE'
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _rise_for_the_people(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.arms.values()[0]._send_joint_trajectory(
[[0.70, -1.9, 0.0, -1.57, 0.0]])
robot.speech.speak(
"Hi there. My Name is Hero. I'm looking for the mates of my operator",
block=False)
return "done"
smach.StateMachine.add("RISE_FOR_THE_PEOPLE",
smach.CBState(_rise_for_the_people),
transitions={"done": "LOCATE_PEOPLE"})
# locate all four people
smach.StateMachine.add('LOCATE_PEOPLE',
LocatePeople(robot, room_id=ROOM_ID),
transitions={
'done': 'RESET_FOR_DRIVING',
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _reset_for_driving(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.speech.speak("Thank you for your attention", block=False)
robot.arms.values()[0]._send_joint_trajectory([
[0.01, -1.9, 0.0, -1.57, 0.0], # Inspect with q0 low
[0.01, 0.0, -1.57, -1.57, 0.0]
]) # Reset
return "done"
smach.StateMachine.add("RESET_FOR_DRIVING",
smach.CBState(_reset_for_driving),
transitions={"done": "GO_BACK_TO_OPERATOR"})
# drive back to the operator to describe the mates
smach.StateMachine.add('GO_BACK_TO_OPERATOR',
NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(robot,
id=OPERATOR_POINT),
radius=0.7,
look_at_designator=ds.EntityByIdDesignator(
robot, id=OPERATOR_POINT)),
transitions={
'arrived': 'REPORT_PEOPLE',
'goal_not_defined': 'REPORT_PEOPLE',
'unreachable': 'WAIT_GO_BACK'
})
smach.StateMachine.add('WAIT_GO_BACK',
WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_BACK_TO_OPERATOR'
})
# check how to uniquely define them # ToDo: make this more interesting
smach.StateMachine.add('REPORT_PEOPLE',
ReportPeople(robot),
transitions={'done': 'done'})
return sm
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()