def main():
keyboard_cmd_listener = rospy.Subscriber("key_commands", String,
callback_keyboard_cmd)
speech_listener = rospy.Subscriber("/speech/output", String,
callback_speech)
rospy.init_node('amigo_demo_executioner')
global dictionary
if rospy.has_param("/nl"):
dictionary['nl'] = rospy.get_param("/nl")
else:
dictionary['nl'] = sentences
if rospy.has_param("/en"):
dictionary['en'] = rospy.get_param("/en")
if rospy.has_param("/language"):
global language
language = rospy.get_param("/language")
print mesg
print_keys()
global robot
#~ if len(sys.argv) > 1:
#~ from test_tools.build_amigo import build_amigo
#~ robot = build_amigo(fake=['base','arms','perception','head', 'worldmodel'])
#~ else:
robot = Amigo(dontInclude=[
"reasoner"
]) #dontInclude=['perception'] is needed for grabbing items
robot.lights.set_color(0, 0, 1)
#import pdb; pdb.set_trace()
rospy.spin()
transitions={
'done': 'RESET',
'continue': 'RESET'
})
smach.StateMachine.add('RESET',
states.ResetED(robot),
transitions={'done': 'WAIT_SAY'})
return sm
if __name__ == '__main__':
rospy.init_node('automatic_side_detection')
from robot_skills.amigo import Amigo
robot = Amigo()
robot.ed.reset()
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('STORE_WAYPOINT',
StoreWaypoint(robot, "kitchen"),
transitions={
'done': 'RESET',
'continue': 'RESET'
})
smach.StateMachine.add('RESET',
states.ResetED(robot),
transitions={'done': 'done'})
# states.startup(setup_statemachine, challenge_name="automatic_side_detection")
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()
robot.reset_all_arms(gripper_timeout=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()
return ActionResult(ActionResult.FAILED,
"Amigo: Failed to move arm to reset position")
amigo.rightArm.wait_for_motion_done()
return ActionResult(ActionResult.SUCCEEDED, "Amigo: Reset arm succeeded")
if __name__ == "__main__":
side = "left"
if len(sys.argv) > 1:
if sys.argv[1] == "--side":
side = sys.argv[2]
""" Test stuff """
rospy.init_node("Test handover motions: Amigo")
amigo = Amigo(wait_services=True)
amigo_reset_arm(amigo, "left")
amigo_reset_arm(amigo, "right")
rospy.loginfo("AMIGO is loaded and will move to the pre-handover pose")
result = amigo_navigate_to_handover_pose(amigo, side)
rospy.loginfo("{0}".format(result.message))
rospy.loginfo("AMIGO will move its arm to the place position")
result = amigo_move_arm_to_place_position(amigo, side)
raw_input("Press enter when SERGIO has its tray under amigo's gripper")
result = amigo_place(amigo, side)
rospy.loginfo("{0}".format(result.message))
