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): rospy.init_node('face_detect_demo') self.wait_for_time() self.lights = robot_api.Lights() rospy.sleep(0.5) self.head = robot_api.FullBodyLookAt( 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 __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 main(): rospy.init_node('base_demo') wait_for_time() argv = rospy.myargv() if len(argv) < 3: print_usage() return command = argv[1] value = float(argv[2]) base = robot_api.Base() if command == 'move': base.go_forward(value) elif command == 'rotate': base.turn(value * math.pi / 180) else: print_usage()
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): # 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.lights = robot_api.Lights() self._listener = tf.TransformListener() self.head = robot_api.FullBodyLookAt(tf_listener=self._listener) self.body = robot_api.Base() self.nav_client = NavWaypoint() self.publisher = rospy.Publisher('face_pose_marker', Marker, queue_size=10) self.num_faces = 0 self.count = 0 self.face_point = None # self.talk_position = None self.look_at = True self.wait_for_time() vision = robot_api.Vision() vision.activate("face_detector", config={"fps": 1}) rospy.sleep(0.5) vision.wait_until_ready(timeout=10) self.face_sub = rospy.Subscriber('vision/results', FrameResults, self.face_callback, queue_size=1)
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__(self): self._server = InteractiveMarkerServer("simple_marker") self._base = robot_api.Base()
def handle_viz_input_forward(input): if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): print('Moving forward 0.5 m') base = robot_api.Base() base.go_forward(0.5, 0.6)
def handle_ccw_input(input): base = robot_api.Base() if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): base.turn(30 * math.pi / 180)
def handle_f_input(input): base = robot_api.Base() if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): base.go_forward(0.5)
def __init__(self, tf_listener=None): super(FullBodyLookAt, self).__init__(tf_listener=tf_listener) self._base = robot_api.Base()
def handle_viz_input_counterclockwise(input): if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): print('Rotating counter-clockwise 90 degrees') base = robot_api.Base() base.turn(1.57, 0.7)
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()
return key speed = .2 turn = 1 def vels(speed, turn): return "currently:\tspeed %s\tturn %s " % (speed, turn) if __name__ == "__main__": settings = termios.tcgetattr(sys.stdin) rospy.init_node('fetch_teleop_key') base = robot_api.Base() x = 0 th = 0 status = 0 count = 0 target_speed = 0 target_turn = 0 control_speed = 0 control_turn = 0 try: print msg print vels(speed, turn) while (1): key = getKey() if key in moveBindings.keys():
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 __init__(self): rospy.init_node('interactive_marker_node') self._server = InteractiveMarkerServer("interactive_marker_interface") # create the interative markers self._forward_marker = InteractiveMarker() self._forward_marker.header.frame_id = 'base_link' self._forward_marker.name = "forward_marker" self._forward_marker.description = "Click to go forward 0.5m" self._forward_marker.pose.position = Point(-0.5,0,-0.5) self._forward_marker.pose.orientation.w = 1 self._cw_marker = InteractiveMarker() self._cw_marker.header.frame_id = 'base_link' self._cw_marker.name = "cw_marker" self._cw_marker.description = "Click to turn 30 degrees clockwise" self._cw_marker.pose.position = Point(-0.5,1,-0.5) self._cw_marker.pose.orientation.w = 1 self._ccw_marker = InteractiveMarker() self._ccw_marker.header.frame_id = 'base_link' self._ccw_marker.name = "ccw_marker" self._ccw_marker.description = "Click to turn 30 degrees counterclockwise" self._ccw_marker.pose.position = Point(-0.5,-1,-0.5) self._ccw_marker.pose.orientation.w = 1 # create a cube for the interactive marker self._forward_box_marker = Marker( type=Marker.CUBE, id=1, scale=Vector3(0.45, 0.45, 0.45), pose=Pose(Point(-0.5,0,0),Quaternion(0,0,0,1)), header=Header(frame_id='base_link', stamp = rospy.Time.now()), color=ColorRGBA(0, 0.5, 0.5, 1.0)) self._cw_box_marker = Marker( type=Marker.CUBE, id=2, scale=Vector3(0.45, 0.45, 0.45), pose=Pose(Point(-0.5,1,0),Quaternion(0,0,0,1)), header=Header(frame_id='base_link', stamp = rospy.Time.now()), color=ColorRGBA(0, 0.5, 0.5, 1.0)) self._ccw_box_marker = Marker( type=Marker.CUBE, id=3, scale=Vector3(0.45, 0.45, 0.45), pose=Pose(Point(-0.5,-1,0),Quaternion(0,0,0,1)), header=Header(frame_id='base_link', stamp = rospy.Time.now()), color=ColorRGBA(0, 0.5, 0.5, 1.0)) # create a control for the interactive marker self._forward_control = InteractiveMarkerControl() self._forward_control.interaction_mode = InteractiveMarkerControl.BUTTON self._forward_control.always_visible = True self._forward_control.markers.append(self._forward_box_marker) self._forward_marker.controls.append(self._forward_control) self._cw_control = InteractiveMarkerControl() self._cw_control.interaction_mode = InteractiveMarkerControl.BUTTON self._cw_control.always_visible = True self._cw_control.markers.append(self._cw_box_marker) self._cw_marker.controls.append(self._cw_control) self._ccw_control = InteractiveMarkerControl() self._ccw_control.interaction_mode = InteractiveMarkerControl.BUTTON self._ccw_control.always_visible = True self._ccw_control.markers.append(self._ccw_box_marker) self._ccw_marker.controls.append(self._ccw_control) self._server.insert(self._forward_marker, self.handle_input) self._server.insert(self._cw_marker, self.handle_input) self._server.insert(self._ccw_marker, self.handle_input) self._server.applyChanges() self._base = robot_api.Base()