def main():
rospy.init_node('head_demo')
wait_for_time()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
command = argv[1]
if command == 'look_at':
if len(argv) < 6:
print_usage()
return
frame_id, x, y, z = argv[2], float(argv[3]), float(argv[4]), float(
argv[5])
head = robot_api.Head()
head.look_at(frame_id,x,y,z)
elif command == 'pan_tilt':
if len(argv) < 4:
print_usage()
return
pan, tilt = float(argv[2]), float(argv[3])
head = robot_api.Head()
head.pan_tilt(pan, tilt)
elif command == 'eyes':
if len(argv) < 3:
print_usage()
return
angle = float(argv[2])
rospy.logerr('Not implemented.')
else:
print_usage()
def __init__(self):
rospy.init_node('ava_voice_cmd')
rospy.on_shutdown(self.cleanup)
self.msg = Twist()
self.social_cues = robot_api.Social_Cues()
self.social_cues.express_neutral()
self.lights = robot_api.Lights()
self.sound_src = kuri_api.SoundSource('AvaVoiceCommand')
self.sound_src_music = kuri_api.SoundSource('AvaVoiceCommand-music')
self._base = robot_api.Base()
self._head = robot_api.Head()
# set head to neutral
self._head.pan_and_tilt(0, -0.1)
self._face_behavior = FaceBehavior()
self._nav_waypoint = NavWaypoint()
self._sleeping = False
# publish to cmd_vel (publishes empty twist message to stop), subscribe to speech output
self._pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.Subscriber('recognizer/asr_output', String, self.speechCb)
# PROMPTING THREAD
#self._kill_prompt_thread = False
#threading.Thread(target=self._prompt_thread).start()
rospy.on_shutdown(self.cleanup)
self._log_set_state(REST)
self._sound_dir = os.path.expanduser(
'~') + "/catkin_ws/src/cse481wi19/ava_custom_audio"
rospy.spin()
def __init__(self):
# search for the person only after the cube is picked up
self._cube_picked = False
self._is_planning = False
# Fetch controls
self._base = robot_api.Base()
self._arm = robot_api.Arm()
self._torso = robot_api.Torso()
self._head = robot_api.Head()
self._fetch_gripper = robot_api.Gripper()
self._move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
self._move_base_client.wait_for_server()
# transformation
self._tf_listener = tf.TransformListener()
self._viz_pub = rospy.Publisher('debug_marker', Marker, queue_size=5)
self._cube_centroid_sub = rospy.Subscriber('/cube_centroid',
PoseStamped,
callback=self.pickup_cube)
self._target_pose_sub = rospy.Subscriber('/target_pose',
TargetPose,
callback=self.goto_target)
rospy.sleep(0.5)
# raise torso
self._torso.set_height(0)
# look down
self._head.pan_tilt(0, 0.8)
def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"):
self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC,
FoodItems, queue_size=10, latch=True)
rospy.loginfo("Given save file path: " + save_file_path)
if os.path.isfile(save_file_path):
rospy.loginfo("File already exists, loading saved positions.")
with open(save_file_path, "rb") as save_file:
try:
self._food_items = pickle.load(save_file)
except EOFError:
# this can be caused if the file is empty.
self._food_items = {}
rospy.loginfo("File loaded...")
else:
rospy.loginfo("File doesn't exist.")
self._food_items = {}
self.__print_food_items__()
self._save_file_path = save_file_path
self.__pub_food_items__()
rospy.loginfo("initializing arm...")
self.arm = robot_api.Arm()
rospy.loginfo("initializing gripper...")
self.gripper = robot_api.Gripper()
rospy.loginfo("initializing head...")
self.head = robot_api.Head()
rospy.loginfo("initializing torso...")
self.torso = robot_api.Torso()
rospy.loginfo("initializing planning scene...")
self.planning_scene = PlanningSceneInterface('base_link')
self.curr_obstacle = None
rospy.loginfo("Starting map annotator...")
# We should expect the nav file given to contain the annotated positions:
# MICROWAVE_LOCATION_NAME - starting location in front of the microwave.
# DROPOFF_LOCATION_NAME - ending dropoff location.
# TODO: Remember to uncomment this section when we get the map working.
# self._map_annotator = Annotator(save_file_path=nav_file_path)
# if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.MICROWAVE_LOCATION_NAME))
# if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.DROPOFF_LOCATION_NAME))
rospy.loginfo("Initialization finished...")
def __init__(self):
self.face_pub = rospy.Publisher('vision/most_confident_face_pos',
PointStamped,
queue_size=10)
self.num_faces_pub = rospy.Publisher('vision/face_count',
Int8,
queue_size=10)
self.lights = robot_api.Lights()
self.head = robot_api.Head()
self.lock = RLock()
self.base = robot_api.Base()
self.facesDemo = FaceInteractionDemo()
rospy.sleep(0.5)
def __init__(self,
feedback_cb=None,
done_cb=None,
use_sounds=True,
move_head=True):
self.head = robot_api.Head()
self.lights = robot_api.Lights()
self.use_sounds = use_sounds
if self.use_sounds:
self.sound_src = kuri_api.SoundSource('Ava')
self.move_head = move_head
self._feedback_cb = feedback_cb
self._done_cb = done_cb
self._sound_dir = os.path.expanduser(
'~') + "/catkin_ws/src/cse481wi19/ava_custom_audio"
def __init__(self):
self.face_pub = rospy.Publisher('vision/most_confident_face_pos',
PointStamped,
queue_size=10)
self.num_faces_pub = rospy.Publisher('vision/face_count',
Int8,
queue_size=10)
self.lights = robot_api.Lights()
self.head = robot_api.Head()
self.lock = RLock()
self.base = robot_api.Base()
self.expressions = robot_api.Expressions()
self.follow = False # Indicates if Luci should follow someone
self.face_exists = False # Indicates if Luci confidently detects a face
self.face_detection_counter = 0
rospy.sleep(
0.5
) # Adding sleep to make sure there's enough to initialize other objects
def __init__(self):
rospy.init_node('face_detect_demo')
self.wait_for_time()
self.lights = robot_api.Lights()
self.head = robot_api.Head(tf_listener=tf.TransformListener())
self.body = robot_api.Base()
self.social_cues = robot_api.Social_Cues(move_head=False, use_sounds=False)
self.publisher = rospy.Publisher('face_pose_marker', Marker, queue_size=10)
self.command_sub = rospy.Subscriber('move_command', Bool, queue_size=1, callback=self.command_callback)
self.face_already_seen = False
self.move_command = False
self.num_faces = 0
vision = robot_api.Vision()
vision.activate("face_detector", config={"fps" : 1})
rospy.sleep(0.5)
vision.wait_until_ready(timeout=10)
vision.face_change.connect(self.face_callback)
if SOCIAL_CUES:
self.social_cues.express_neutral()
rospy.spin()
def main():
rospy.init_node('gripper_teleop_node')
# raise torso
torso = robot_api.Torso()
torso.set_height(0.4)
# title head to look at the ground
head = robot_api.Head()
head.pan_tilt(0, math.pi / 8)
arm = robot_api.Arm()
gripper = robot_api.Gripper()
# im_server = InteractiveMarkerServer('gripper_im_server')
# teleop = GripperTeleop(arm, gripper, im_server)
# teleop.start()
auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server')
auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server)
auto_pick.start()
rospy.spin()
def __init__(self):
# Setup
self.tag_reader = ArTagReader()
self.ar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, callback=self.tag_reader.callback)
self.odom_sub = rospy.Subscriber("odom", Odometry, callback=self.saveCurrentPose)
# self.move_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
self.ac = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.head = robot_api.Head()
self.base = robot_api.Base()
self.expressions = robot_api.Expressions()
self.tf = tf.TransformListener()
self.currentPoseStamped = None
self.store = SequenceStore()
self.command_sequence = []
print("Waiting for ActionClient Server...")
self.ac.wait_for_server()
print("Waiting for AR markers...")
while len(self.tag_reader.markers) is 0: pass
print("Waiting to read current pose...")
while self.currentPoseStamped is None: pass
# Set current tag for saving (first one we see by default)
self.in_progress_seq_tag = self.tag_reader.markers[0].id
def __init__(self):
self.face_pub = rospy.Publisher('vision/most_confident_face_pos', PointStamped, queue_size=10)
self.num_faces_pub = rospy.Publisher('vision/face_count', Int8, queue_size=10)
self.lights = robot_api.Lights()
self.head = robot_api.Head()
self.lock = RLock()
self.base = robot_api.Base()
self.lastFacePos = None
self.vision = robot_api.Vision()
# Activate Vision API's Face Detector
self.vision.activate("face_detector", config={"fps": 3})
rospy.sleep(0.5)
self.vision.wait_until_ready(timeout=10)
self.vision.req_mods([["activate", "face_detector", {"fps": 3}, {"skip_ratio": 3}]], [])
# Trigger callback
rospy.Subscriber('vision/results', FrameResults, self.callback)
rospy.Subscriber('vision/most_confident_face_pos', PointStamped, self.onFacePos)
rospy.Subscriber('come_here_kuri', Int8, self.navigateTo)
rospy.sleep(0.5)
rospy.spin()
def init_robot(self):
"""
0a. Move torso to default position
0b. reset head
0c. open gripper
0d. move arm to starting pos (start_pos.pkl)
"""
rospy.loginfo("STARTING SEGMENT 1")
rospy.loginfo("0a. Move torso to default position")
torso = robot_api.Torso()
torso.set_height(0.4)
rospy.sleep(2)
rospy.loginfo("0b. reset head")
head = robot_api.Head()
head.pan_tilt(-0.1, 0.57)
rospy.sleep(2)
rospy.loginfo("0c. open gripper")
self.gripper.open()
rospy.loginfo("0d. Move arm to starting pos")
self.__load_program_and_run__("start_pos.pkl", id)
rospy.sleep(1.5)
def __init__(self):
self._head = robot_api.Head(None)
self._lights = robot_api.Lights()
def main():
print("Initializing...")
rospy.init_node('project_master_node')
wait_for_time()
found_object = False
picked_object = False
face_detected = False
print("Initializing base...")
base = robot_api.Base()
print("Initializing head...")
head = robot_api.Head()
print("Initializing gripper...")
gripper = robot_api.Gripper()
gripper.open()
print("Initializing torso...")
torso = robot_api.Torso()
torso.set_height(0)
print("Initializing arm motion planner...")
arm = robot_api.Arm()
arm.move_to_initial_pose()
arm_planner = ArmMotionPlanner(arm, gripper)
print("Initializing object detector...")
object_reader = ObjectMarkerReader()
object_marker_sub = rospy.Subscriber('object_markers',
Marker,
callback=object_reader.callback)
print("Initializing face detector...")
face_detector = FaceDetector()
face_marker_sub = rospy.Subscriber('face_marker',
Marker,
callback=face_detector.callback)
print("Initializing navigation...")
navigator = Navigator()
print("Initialized")
while not rospy.is_shutdown():
# Part 1
# Look for objects
# If not found, turn around and repeat
head.pan_tilt(0.0, 0.85)
rospy.sleep(
1
) # wait extra time after tilting for the first point cloud to arrive
# #
head.pan_tilt(0.0, 0.9)
TURN_STEPS = 10
angular_distance = 2 * math.pi / TURN_STEPS
object_marker = None
while True:
print("Looking for objects...")
rospy.sleep(8)
# time.sleep(2)
if object_reader.object_detected():
print("Found object")
object_marker = object_reader.get_object()
break
print("Could not find object. turning around...")
base.turn(angular_distance)
print("Retrying...")
# Part 2
# Attempt to pickup object
# If success, go to next step
# If failure, restart from Part 1
if object_marker is None:
print("ERROR: Detected Object Gone...")
return
arm.move_to_hold_pose() # synchronized
picked_object = arm_planner.pick_up(object_marker)
if not picked_object:
print("Could not pick the object. Restart from looking objects...")
continue
print("Picked object")
arm.move_to_hold_pose()
# Part 3
# Look for face
# If found, move to face location
# If not found, turn around and repeat
head.pan_tilt(0.0, 0.0)
face_detected = False
face_location = None
max_dist = 2.5
while True:
print("Looking for people...")
rospy.sleep(5)
if face_detector.face_detected():
print("Found person")
face_marker = face_detector.get_face()
face_location = transform_marker(navigator, face_marker)
if face_location.pose.position.x < max_dist:
break
print("Person out of range...")
else:
print("Could not find face...")
print("Turning around...")
base.turn(angular_distance)
print("Retrying...")
# Part 4
# Go to the face location
# Wait for 5 seconds and open the gripper
# Go to Part 1
while True:
print("Moving to person...")
reached = go_forward(navigator, base, face_location)
# reached = go_to(navigator, face_location)
if not reached:
print("Could not reach the face. Retrying...")
# face_location.pose.position.x -= 0.05
# print("updated face_location_baselink x:", face_location.pose.position.x)
continue
print("Delivering object...")
time.sleep(5)
gripper.open()
print("Object delivered")
# arm.move_to_initial_pose()
print("Demo lite round 1 complete...")
return
def wiggle_head(self):
head = robot_api.Head()
head.pan_tilt(-0.1, 0.5)
rospy.sleep(2)
head.pan_tilt(-0.1, 0.57)
def main():
rospy.init_node("annotator_node")
print("starting node")
wait_for_time()
print("finished node")
print("starting node 2")
listener = tf.TransformListener()
rospy.sleep(1)
print("finished node 2")
reader = ArTagReader()
sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, callback=reader.callback)
print('finished subscribing to ARTag reader')
controller_client = actionlib.SimpleActionClient('query_controller_states', QueryControllerStatesAction)
print('passed action lib')
arm = robot_api.Arm()
print('got arm')
gripper = robot_api.Gripper()
print('got gripper')
torso = robot_api.Torso()
print('got torso')
head = robot_api.Head()
print('got head')
server = roboeats.RoboEatsServer()
print_intro()
program = Program(arm, gripper, head, torso)
print("Program created.")
running = True
food_id = None
while running:
user_input = raw_input(">>>")
if not user_input:
# string is empty, ignore
continue
args = user_input.split(" ")
cmd = args[0]
num_args = len(args) - 1
if cmd == "create":
program.reset()
print("Program created.")
elif cmd == "save-pose" or cmd == "sp":
if num_args >= 1:
try:
if num_args == 1:
alias = None
frame = args[1]
elif num_args == 2:
alias = args[1]
frame = args[2]
if frame == "base_link":
(pos, rot) = listener.lookupTransform(frame, "wrist_roll_link", rospy.Time(0))
ps = PoseStamped()
ps.header.frame_id = frame
ps.pose.position.x = pos[0]
ps.pose.position.y = pos[1]
ps.pose.position.z = pos[2]
ps.pose.orientation.x = rot[0]
ps.pose.orientation.y = rot[1]
ps.pose.orientation.z = rot[2]
ps.pose.orientation.w = rot[3]
else:
# while True:
transform = listener.lookupTransform(frame, "base_link", rospy.Time(0))
rot = transform[1]
x, y, z, w = rot
print("stage 1: " + pos_rot_str(transform[0], transform[1]))
tag_T_base = create_transform_matrix(transform)
user_input = raw_input("saved base relative to the frame, move the arm and press enter when done")
transform = listener.lookupTransform("base_link", "wrist_roll_link", rospy.Time(0))
print("stage 2: " + pos_rot_str(transform[0], transform[1]))
base_T_gripper = create_transform_matrix(transform)
ans = np.dot(tag_T_base, base_T_gripper)
ans2 = tft.quaternion_from_matrix(ans)
ps = PoseStamped()
ps.pose.position.x = ans[0, 3]
ps.pose.position.y = ans[1, 3]
ps.pose.position.z = ans[2, 3]
ps.pose.orientation.x = ans2[0]
ps.pose.orientation.y = ans2[1]
ps.pose.orientation.z = ans2[2]
ps.pose.orientation.w = ans2[3]
ps.header.frame_id = frame
print("Saved pose: " + ps_str(ps))
program.add_pose_command(ps, alias)
print("done")
except Exception as e:
print("Exception:", e)
else:
print("No frame given.")
elif cmd == "save-open-gripper" or cmd == "sog":
program.add_open_gripper_command()
gripper.open()
elif cmd == "save-close-gripper" or cmd == "scg":
program.add_close_gripper_command()
gripper.close()
elif cmd == "alias":
if num_args == 2:
i = int(args[1])
alias = args[2]
program.set_alias(i, alias)
else:
print("alias requires <i> <alias>")
elif cmd == "delete" or cmd == "d":
program.delete_last_command()
elif cmd == "replace-frame" or cmd == "rf":
if num_args == 2:
alias = args[1]
new_frame = args[2]
program.replace_frame(alias, new_frame)
else:
print("Expected 2 arguments, got " + str(num_args))
elif cmd == "run-program" or cmd == "rp":
program.run(None)
relax_arm(controller_client)
elif cmd == "savef":
if len(args) == 2:
try:
fp = args[1]
if program is None:
print("There is no active program currently.")
else:
program.save_program(fp)
except Exception as e:
print("Failed to save!\n", e)
else:
print("No save path given.")
elif cmd == "loadf":
if len(args) == 2:
fp = args[1]
if os.path.isfile(fp):
print("File " + fp + " exists. Loading...")
with open(fp, "rb") as load_file:
program.commands = pickle.load(load_file)
print("Program loaded...")
program.print_program()
else:
print("No frame given.")
elif cmd == "print-program" or cmd == "ls" or cmd == "list":
program.print_program()
elif cmd == "relax":
relax_arm(controller_client)
elif cmd == "unrelax":
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
controller_client.wait_for_result()
print("Arm is now unrelaxed.")
elif cmd == "tags":
for frame in reader.get_available_tag_frames():
print("\t" + frame)
elif cmd == "help":
print_commands()
elif cmd == "stop":
running = False
elif cmd == "torso":
if num_args == 1:
height = float(args[1])
program.add_set_height_command(height)
torso.set_height(height)
else:
print("missing <height>")
elif cmd == "head":
if num_args == 2:
pan = float(args[1])
tilt = float(args[2])
program.add_set_pan_tilt_command(pan, tilt)
head.pan_tilt(pan, tilt)
else:
print("missing <pan> <tilt>")
elif cmd == "init":
server.init_robot()
elif cmd == "attachl":
server.attach_lunchbox()
elif cmd == "detachl":
server.remove_lunchbox()
elif cmd == "obstacles1":
server.start_obstacles_1()
elif cmd == "obstacles2":
server.start_obstacles_2()
elif cmd == "clear-obstacles":
server.clear_obstacles()
elif cmd == "segment1a":
server.start_segment1a(food_id)
elif cmd == "segment1b":
server.start_segment1b(food_id)
elif cmd == "segment2":
server.start_segment2(food_id)
elif cmd == "segment3":
server.start_segment3(food_id)
elif cmd == "segment4":
server.start_segment4(food_id)
elif cmd == "all-segments":
rqst = StartSequenceRequest()
rqst.id = food_id
server.handle_start_sequence(rqst)
elif cmd == "set-food-id":
if num_args == 1:
food_id = int(args[1])
else:
print("Requires <food_id>")
elif cmd == "addf":
if num_args == 3:
rqst = CreateFoodItemRequest()
rqst.name = args[1]
rqst.description = args[2]
rqst.id = int(args[3])
server.handle_create_food_item(rqst)
else:
print("Requires <name> <description> <id>")
elif cmd == "list-foods":
server.__print_food_items__()
else:
print("NO SUCH COMMAND: " + cmd)
print("")
def __init__(self):
self._head = robot_api.Head()
def main():
rospy.init_node('head_demo')
wait_for_time()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
command = argv[1]
head = robot_api.Head()
expression = robot_api.Expressions()
sound_source = robot_api.SoundSource('Test')
if command == 'look_at':
if len(argv) < 6:
print_usage()
return
frame_id, x, y, z = argv[2], float(argv[3]), float(argv[4]), float(
argv[5])
# Setup Header
h = Header()
h.frame_id = frame_id
h.stamp = rospy.Time().now()
# Setup Point
p = Point()
p.x = x
p.y = y
p.z = z
# Point Stamped
ps = PointStamped()
ps.header = h
ps.point = p
result = head.look_at(ps, True)
if result: print("Success")
else: print("Point out of range")
elif command == 'pan_tilt':
if len(argv) < 4:
print_usage()
return
pan, tilt = float(argv[2]), float(argv[3])
head.pan_and_tilt(pan, tilt)
elif command == 'eyes':
if len(argv) < 3:
print_usage()
return
angle = float(argv[2])
head.eyes_to(angle)
elif command == 'nod_head':
expression.nod_head()
elif command == 'shake_head':
expression.shake_head()
elif command == 'be_happy':
expression.be_happy()
elif command == 'be_sad':
expression.be_sad()
elif command == 'be_neutral':
expression.be_neutral()
elif command == 'play_sound':
# TODO Factor out sound stuff into different demo file
sound = sound_source.play('/home/team1/catkin_ws/src/sound_effects/Monkey-Screech.wav')
rospy.sleep(0.5)
sound = sound_source.play('/home/team1/catkin_ws/src/sound_effects/Monkey-Screech.wav')
rospy.sleep(0.5)
sound = sound_source.play('/home/team1/catkin_ws/src/sound_effects/Monkey-Screech.wav')
rospy.sleep(1)
sound_source.cancel(sound)
else:
print_usage()
def main():
print("Initializing...")
rospy.init_node('project_master_node')
wait_for_time()
found_object = False
picked_object = False
face_detected = False
print("Initializing base...")
base = robot_api.Base()
print("Initializing head...")
head = robot_api.Head()
print("Initializing gripper...")
gripper = robot_api.Gripper()
print("Initializing torso...")
torso = robot_api.Torso()
torso.set_height(0.1)
print("Initializing arm motion planner...")
arm = robot_api.Arm()
arm.move_to_initial_pose()
arm_planner = ArmMotionPlanner(arm, gripper)
print("Initializing object detector...")
object_reader = ObjectMarkerReader()
object_marker_sub = rospy.Subscriber('object_markers', Marker, callback=object_reader.callback)
print("Initializing face detector...")
face_detector = FaceDetector()
face_marker_sub = rospy.Subscriber('face_locations', PoseStamped, callback=face_detector.callback)
print("Initializing navigation...")
navigator = Navigator()
rospy.sleep(1)
rate = rospy.Rate(0.5)
print("Initialized")
while not rospy.is_shutdown():
# Part 1
# Look for objects
# If found, move to object location
# If not found, roam around and repeat
while not found_object:
print("Looking for objects...")
head.pan_tilt(0.0, 1.0)
time.sleep(10)
rate.sleep()
if object_reader.object_detected():
found_object = True
print("Found object")
else:
print("Could not find object. Moving around...")
move_random(navigator, base)
time.sleep(2)
rate.sleep()
print("Retrying...")
time.sleep(3)
rate.sleep()
# Part 2
# Pickup object
# Goto the object location and attempt to pick it up
# If success, go to next step
# If failure, go to Part 1
while found_object and not picked_object:
object = object_reader.get_object()
if object is None:
print("Detected Object Gone...")
found_object = False
arm.move_to_initial_pose()
continue
target_pose = transform_marker(navigator, object)
reached = True
if target_pose == None:
reached = True
else:
reached = navigator.goto(target_pose, MOVE_TIMEOUT)
if not reached:
print("Could not reach the object. Retrying...")
continue
else:
head.pan_tilt(0.0, 1.0)
time.sleep(10)
rate.sleep()
object_to_pick = object_reader.get_object(object.id)
if object_to_pick is None:
print("Object Gone...")
found_object = False
arm.move_to_initial_pose()
continue
print(object_to_pick)
arm.move_to_hold_pose()
picked_object = arm_planner.pick_up(object_to_pick)
if not picked_object:
print("Could not pick the object. Retrying...")
found_object = False
arm.move_to_initial_pose()
else:
print("Picked object")
arm.move_to_hold_pose()
# Part 3
# Look for face
# If found, move to face location
# If not found, roam around and repeat
while found_object and picked_object and not face_detected:
print("Looking for people...")
head.pan_tilt(0.0, 0.0)
time.sleep(3)
if face_detector.face_detected():
face_detected = True
print("Found person")
else:
print("Could not find face. Moving around...")
move_random(navigator, base)
time.sleep(2)
rate.sleep()
print("Retrying...")
# Part 4
# Go to the face location
# Wait for 5 seconds and open the gripper
# Go to Part 1
if found_object and picked_object and face_detected:
print("Moving to person...")
face_location = transform_marker(navigator, face_detector.get_face())
face_detected = navigator.goto(face_location, MOVE_TIMEOUT)
reached = navigator.goto(face_location, MOVE_TIMEOUT)
if not reached:
print("Count not reach the face. Retrying...")
continue
else:
print("Delivering object...")
time.sleep(3)
gripper.open()
found_object = False
picked_object = False
face_detected = False
print("Object delivered")
arm.move_to_initial_pose()
rate.sleep()
def __init__(self):
self._torso = robot_api.Torso()
self._head = robot_api.Head()
self._grip = robot_api.Gripper()
self._arm = robot_api.Arm()