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()
