def main(): rospy.init_node('hallucinated_reach') wait_for_time() torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) start = PoseStamped() start.header.frame_id = 'base_link' start.pose.position.x = 0.5 start.pose.position.y = 0.5 start.pose.position.z = 0.75 arm = robot_api.Arm() arm.move_to_pose(start) reader = ArTagReader() sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers, callback=reader.callback ) # Subscribe to AR tag poses, use reader.callback while len(reader.markers) == 0: rospy.sleep(0.1) for marker in reader.markers: # get the pose to move to pose = marker.pose pose.header.frame_id = marker.header.frame_id error = arm.move_to_pose(pose) if error is None: rospy.loginfo('Moved to marker {}'.format(marker.id)) return else: rospy.logwarn('Failed to move to marker {}'.format(marker.id)) rospy.logerr('Failed to move to any markers!')
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 main(): rospy.init_node('torso_demo') wait_for_time() argv = rospy.myargv() if len(argv) < 2: print_usage() return height = float(argv[1]) torso = robot_api.Torso() torso.set_height(height)
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 main(): rospy.init_node('arm_demo') wait_for_time() argv = rospy.myargv() DISCO_POSES = [[1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0], [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [-1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0], [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]] torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() for vals in DISCO_POSES: arm.move_to_joints(robot_api.ArmJoints.from_list(vals))
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 main(): rospy.init_node('cart_arm_demo') wait_for_time() # argv = rospy.myargv() pose1 = Pose(Point(0.042, 0.384, 1.826), Quaternion(0.173, -0.693, -0.242, 0.657)) pose2 = Pose(Point(0.047, 0.545, 1.822), Quaternion(-0.274, -0.701, 0.173, 0.635)) ps1 = PoseStamped() ps1.header.frame_id = 'base_link' ps1.pose = pose1 ps2 = PoseStamped() ps2.header.frame_id = 'base_link' ps2.pose = pose2 gripper_poses = [ps1, ps2] # set torso to max height torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() # register shutdown method def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) # repeatedly moves the gripper between two poses: while True: for pose in gripper_poses: error = arm.move_to_pose(pose) if error is not None: rospy.logerr(error) # give the arm some time to settle down rospy.sleep(1)
def main(): rospy.init_node('arm_motion_planner_demo') wait_for_time() # argv = rospy.myargv() # if len(argv) < 2: # print_usage() # return # name = argv[1] # rospy.sleep(0.5) torso = robot_api.Torso() torso.set_height(0) arm = robot_api.Arm() gripper = robot_api.Gripper() reader = MarkerReader() marker_sub = rospy.Subscriber('object_markers', Marker, callback=reader.callback) rospy.sleep(2) arm_planner = ArmMotionPlanner(arm, gripper) rate = rospy.Rate(10) while not rospy.is_shutdown(): # print(reader.obj_marker) obj_list = copy.deepcopy(reader.obj_markers) # obj_list = set(copy.deepcopy(obs_list)) for obj_marker_id in obj_list: # for obs_marker in obs_list: # print(obs_marker) # print() # print(obj_marker) # obs_list.remove(obj_marker) arm_planner.pick_up(obj_list[obj_marker_id]) # obs_list.add(obj_marker) # arm_planner.pick_up(reader.obj_marker) rate.sleep()
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 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 main(): rospy.init_node('arm_obstacle_demo') wait_for_time() # argv = rospy.myargv() planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray') # Create table obstacle table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # add orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 # # set torso to max height torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() # register shutdown method def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 20, 'num_planning_attempts': 10, 'replan': True } error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') # attach an object if pose1 is successfully reached frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() # close the gripper # gripper = robot_api.Gripper() # gripper.close() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') # At the end of the program planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray')
def main(): rospy.init_node('load_exec_program_demo') wait_for_time() argv = rospy.myargv() if len(argv) < 2: print_usage() return name = argv[1] rospy.sleep(0.5) goal = QueryControllerStatesGoal() state = ControllerState() state.name = 'arm_controller/follow_joint_trajectory' state.state = ControllerState.RUNNING goal.updates.append(state) control_client = actionlib.SimpleActionClient( "/query_controller_states", robot_controllers_msgs.msg.QueryControllerStatesAction) # TODO: Wait for server control_client.wait_for_server() control_client.send_goal(goal) control_client.wait_for_result() torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() program = gripper_program.GripperProgram() gripper = robot_api.Gripper() tf_listener = tf.TransformListener() reader = ArTagReader() marker_sub = rospy.Subscriber( 'ar_pose_marker', AlvarMarkers, callback=reader.callback ) # Subscribe to AR tag poses, use reader.callback # rate = rospy.Rate(10) # rate.sleep() rospy.sleep(2) actions = program.load_program(name) if actions is None: exit(1) for action in actions: if action.gripper_open: gripper.open() else: gripper.close(robot_api.Gripper.MAX_EFFORT) pose_stamped = action.pose_stamped id = pose_stamped.header.frame_id # print(reader.markers) if id not in reader.markers and id != 'base_link': print("Frame " + id + " does not exits.") exit(1) if id in reader.markers: marker_tf = pose_to_transform(reader.markers[id].pose.pose) # print(reader.markers[id].pose) goal_tf = pose_to_transform(pose_stamped.pose) result_tf = np.dot(marker_tf, goal_tf) pose_stamped.header.frame_id = 'base_link' pose_stamped.pose = transform_to_pose(result_tf) # pose_stamped.pose = reader.markers[id].pose.pose # print(pose_stamped) error = arm.move_to_pose(pose_stamped) if error is not None: rospy.logerr(error) rospy.spin()
def __init__(self): self._torso = robot_api.Torso() self._head = robot_api.Head() self._grip = robot_api.Gripper() self._arm = robot_api.Arm()
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 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()