def play_animation(robot: cozmo.robot.Robot, anim_trig, body = False, para = False):
# anim_trig = cozmo.anim.Triggers."Name of trigger" (this is an object)
# Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers" for animations' triggers
# Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.robot.html#cozmo.robot.Robot.play_anim_trigger" for playing the animation
robot.play_anim_trigger(anim_trig,loop_count = 1, in_parallel = para,
num_retries = 0, use_lift_safe = False,
ignore_body_track = body, ignore_head_track=False,
ignore_lift_track=False).wait_for_completed()
def level(robot: cozmo.robot.Robot): global LIFT_POSITION init(robot) robot.set_lift_height(0.8).wait_for_completed() LIFT_POSITION = robot._lift_position.ratio setBackpackColors() robot.set_lift_height(0.0).wait_for_completed() celebrate(robot) return
def find_face(robot: cozmo.robot.Robot):
# Look around for a face
behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.FindFaces)
try:
face_to_follow = robot.world.wait_for_observed_face(timeout=5)
except:
face_to_follow = None
# Stop looking around
behavior.stop()
if(face_to_follow):
turn_action = robot.turn_towards_face(face_to_follow).wait_for_completed()
def init_cubes(robot: cozmo.robot.Robot):
global cube1,cube2,cube3,ev_cube,ev_id,ev_command,ev_light
cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip
cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart
cube3 = robot.world.get_light_cube(LightCube3Id) # looks like the letters 'ab' over 'T'
# Make sure each cube is off
switch_cubes_off()
# Add listener to control initialisation
handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped, handle_tap_init)
seq = ['red','green','blue']
light_seq = [cozmo.lights.red_light,cozmo.lights.green_light,cozmo.lights.blue_light]
cube_seq = [cube1,cube2,cube3]
comm_seq = ['g','c','s'] # greater,correct,smaller
for i in range(0,3):
print('Tap the ',seq[i],' cube')
switch_cube_on(cube_seq[i],light_seq[i])
while(True):
if(ev_id):
# Map event to cube
ev_cube[ev_id] = cube_seq[i]
ev_command[ev_id] = comm_seq[i]
ev_light[ev_id] = light_seq[i]
make_selected_cube_blink(ev_cube[ev_id],light_seq[i])
switch_cubes_off()
ev_id = 0
break
handler.disable()
def get_answer_from_cubes(robot: cozmo.robot.Robot):
global cube1,cube2,cube3,ev_cube,ev_id,ev_command,ev_light
timeOut = 30
timer = 0
switch_cubes_on()
handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped, handle_tapped)
while(True):
time.sleep(0.5)
timer += 0.5
if(ev_id):
switch_cubes_off()
make_selected_cube_blink(ev_cube[ev_id],ev_light[ev_id])
local_id = ev_id
ev_id = 0 # Deactivate selection
handler.disable()
return ev_command[local_id]
elif(timer > timeOut):
handler.disable()
return 'timeOut'
def cozmo_program(robot: cozmo.robot.Robot):
# Play an animation via a Trigger - see:
# http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers
# for a list of available triggers.
# A trigger can pick from several appropriate animations for variety.
# robot.play_anim_trigger(cozmo.anim.Triggers.DizzyReactionSoft).wait_for_completed()
# robot.play_anim_trigger(cozmo.anim.Triggers.DizzyReactionMedium).wait_for_completed()
# robot.play_anim_trigger(cozmo.anim.Triggers.DizzyReactionHard).wait_for_completed()
robot.start_behavior(cozmo.behavior.BehaviorTypes.StackBlocks).wait_for_completed();
robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpIdle).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpLeftHanging).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpSuccess).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession).wait_for_completed()
# Play an animation via its Name.
# Warning: Future versions of the app might change these, so for future-proofing
# we recommend using play_anim_trigger above instead.
# See the remote_control_cozmo.py example in apps for an easy way to see
# the available animations.
robot.play_anim(name="id_poked_giggle").wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
global isTakingPicture
global targetObject
targetObject = sys.argv[1]
if os.path.exists('photos'):
shutil.rmtree('photos')
if not os.path.exists('photos'):
os.makedirs('photos')
robot.say_text(f"Somebody lost the {targetObject}. Don't worry, I'll find it.").wait_for_completed()
# reset Cozmo's arms and head
robot.set_head_angle(degrees(10.0)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.add_event_handler(cozmo.world.EvtNewCameraImage, on_new_camera_image)
while not discoveredObject:
isTakingPicture = False
robot.turn_in_place(degrees(45)).wait_for_completed()
isTakingPicture = True
time.sleep(2)
isTakingPicture = False
if discoveredObject:
robot.drive_straight(distance_mm(200), speed_mmps(300)).wait_for_completed()
robot.say_text(f"Oh yay! I've found the {targetObject}").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin).wait_for_completed()
async def loopPath(self, robot: cozmo.robot.Robot):
if robot.is_on_charger:
await robot.drive_off_charger_contacts().wait_for_completed()
self.initialPose = robot.pose
self.pathPoseTrack = self.track.getPathPoseTrack(self.forwardSpeed)
self.driveOnRoad = True
self.stopped = False
self.waitForOrder = False
self.waitForAnimation = False
self.offsetPixel = 0.0
self.deliveryCount = 0
self.attentionCount = 0
print("start drive")
## await robot.drive_wheels(FORWARD_SPEED, FORWARD_SPEED)
print(self.pathPoseTrack.distance)
await robot.drive_straight(
distance_mm(self.pathPoseTrack.distance),
speed_mmps(self.forwardSpeed)).wait_for_completed()
print(self.pathPoseTrack.edge.end.id)
while not self.stopped:
# basic update of straight drive
# picking next edge if the current is finished
if self.driveOnRoad:
## self.pathPoseTrack.update(FRAME_DURATION, FORWARD_SPEED)
# finish this path, because drive_straight is used and waited to finish
self.pathPoseTrack.update(999, self.forwardSpeed)
# did the last auto delivery
if self.deliveryCount > self.maxDelivery:
await robot.play_anim_trigger(random.choice(ATTENTION_TRIGGERS)
).wait_for_completed()
self.attentionCount = self.attentionCount + 1
# end of the path
elif self.pathPoseTrack.consumeRouteEndSignal():
self.driveOnRoad = False
# stop before turn
robot.stop_all_motors()
# turning is included in go to pose
self.pathPoseTrack.consumeEdgeChangeSignal()
# go to loading area and drop cube
# lift cube, go back to road
bldgId = self.pathPoseTrack.edge.start.id
if bldgId != "GA":
await self.deliverItem(
robot, self.pathPoseTrack.edge.start,
self.pathPoseTrack.path.lastTurnRight)
else:
await self.backInGarage(robot, False)
self.stopped = True
if self.stopped:
break
# continue auto delivery
if self.deliveryCount <= self.maxDelivery:
await self.depart(robot)
print("Move towards: %s" % self.pathPoseTrack.edge.end.id)
# at any road intersection
elif self.pathPoseTrack.consumeEdgeChangeSignal():
self.driveOnRoad = False
diff = self.pathPoseTrack.angleDiff
angleAbs = self.pathPoseTrack.angleAbs
# a turn is about to be made
if diff < -0.1 or diff > 0.1:
# stop before turn
robot.stop_all_motors()
# make a turn
angle = radians(angleAbs +
self.initialPose.rotation.angle_z.radians -
robot.pose.rotation.angle_z.radians)
await robot.turn_in_place(angle).wait_for_completed()
print("turn of angle: ", angle.degrees)
# restart motion
## await robot.drive_wheels(FORWARD_SPEED, FORWARD_SPEED)
anim_done = False
while anim_done is False:
try:
await robot.drive_straight(
distance_mm(self.pathPoseTrack.distance),
speed_mmps(
self.forwardSpeed)).wait_for_completed()
anim_done = True
except cozmo.exceptions.RobotBusy:
await asyncio.sleep(0.1)
self.driveOnRoad = True
print("Move towards: %s" % self.pathPoseTrack.edge.end.id)
# let Cozmo drive straight for a short time before updates
await asyncio.sleep(FRAME_DURATION)
robot.stop_all_motors()
def cozmo_program(robot: cozmo.robot.Robot):
for _ in range(4):
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
# falls der Lift oben ist wird er runter geholt
robot.set_lift_height(0.0).wait_for_completed()
# dreht sich um 45° nach links
robot.turn_in_place(degrees(45)).wait_for_completed()
# Lift geht hoch und wieder runter (at 5 radians per second)
robot.move_lift(5)
time.sleep(0.5)
robot.move_lift(-5)
time.sleep(0.5)
# dreht sich um 90° nach rechts
robot.turn_in_place(degrees(-90)).wait_for_completed()
# Lift geht hoch und wieder runter (at 5 radians per second)
robot.move_lift(5)
time.sleep(0.5)
robot.move_lift(-5)
time.sleep(0.5)
# dreht sich um 45° nach links
robot.turn_in_place(degrees(45)).wait_for_completed()
# Lift geht hoch und wieder runter (at 5 radians per second)
robot.move_lift(5)
time.sleep(0.5)
robot.move_lift(-5)
time.sleep(0.5)
# faehrt 150 millimeters rueckwaerts mit 50 millimeters-per-second.
robot.drive_straight(distance_mm(-150),
speed_mmps(150)).wait_for_completed()
# dreht sich um 45° nach links
robot.turn_in_place(degrees(45)).wait_for_completed()
# Lift geht hoch und wieder runter (at 5 radians per second)
robot.move_lift(5)
time.sleep(0.5)
robot.move_lift(-5)
time.sleep(0.5)
# dreht sich um 90° nach rechts
robot.turn_in_place(degrees(-90)).wait_for_completed()
# Lift geht hoch und wieder runter (at 5 radians per second)
robot.move_lift(5)
time.sleep(0.5)
robot.move_lift(-5)
time.sleep(0.5)
# dreht sich um 45° nach links
robot.turn_in_place(degrees(45)).wait_for_completed()
# Lift geht hoch und wieder runter (at 5 radians per second)
robot.move_lift(5)
time.sleep(0.5)
robot.move_lift(-5)
time.sleep(0.5)
# faehrt 150 millimeters vorwaerts mit 50 millimeters-per-second.
robot.drive_straight(distance_mm(150),
speed_mmps(150)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
global angle
angle = 25.
robot.set_head_angle(degrees(angle)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
robot.camera.enable_auto_exposure = True
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3'
frame = os.path.join(directory, "current.jpeg")
print("Starting Tensorflow...")
with tf.Session() as sess:
print("Session successfully started")
model = load_model('modelv1.07-0.96.hdf5')
while True:
global X, Y, W, H
global result
X = 245.
Y = 165.
W = 150.
H = 150.
gt = [X, Y, W, H]
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
frame = os.path.join(directory, "current.jpeg")
result = 0
dog_counter = 0
cat_counter = 0
background_counter = 0
next_state = 0
current_state = 0 #Background: 0, Cat:1, Dog:2
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
show_frame(np.asarray(Image.open(frame)), [900.,900.,900.,900.], 1)
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
next_state = np.argmax(result)
print('Arg max: ',next_state)
# Initial Current State is Background
if current_state == 0:
print('Background')
if next_state == 1: # Detected a Cat
current_state = 1 # Transition to Cat State
background_counter = 0
cat_counter = 1
dog_counter = 0
elif next_state == 2: # Detected a Dog
current_state = 2 # Transition to Dog state
background_counter = 0
cat_counter = 0
dog_counter = 1
# Current State is Cat
elif current_state == 1:
print('\t\t\t\t\t\tCat')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
cat_counter = 0
elif next_state == 1: # Detected Cat itself
cat_counter +=1
if cat_counter >= 30:
print('Cozmo sees a cat')
dense = model.get_layer('dense').get_weights()
weights = dense[0].T
testing_counter = 0
detected_centroid = 0
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
frame_average = 2
frame_count = 0
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
kernels = out_relu.reshape(7,7,1280)
final = np.dot(kernels,weights[result[0].argmax()])
final1 = array_to_img(final.reshape(7,7,1))
final1 = final1.resize((224,224), Image.ANTIALIAS)
box = img_to_array(final1).reshape(224,224)
#box = cv2.blur(box,(30,30))
temp = (box > box.max()*.8) *1
temp_adjusted = np.ndarray(shape=np.shape(temp), dtype=np.dtype(np.uint8))
temp_adjusted[:,:] = np.asarray(temp)*255
contours, hierarchy = cv2.findContours(temp_adjusted, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
contours = np.array(contours)
max_area = [0,0] # contours index and area
for index, contour in enumerate(contours):
if(max_area[1]< len(contour)):
max_area = [index,len(contour)]
contours_adjusted = contours[max_area[0]].squeeze(axis=1).T
xmin = contours_adjusted[0].min() * (640./224.)
ymin = contours_adjusted[1].min() * (480./224.)
xmax = contours_adjusted[0].max() * (640./224.)
ymax = contours_adjusted[1].max() * (480./224.)
if result[0].argmax() == 1:
# Frame smoothing
frame_count = frame_count + 1
xmin_avg = xmin_avg + xmin
xmax_avg = xmax_avg + xmax
ymin_avg = ymin_avg + ymin
ymax_avg = ymax_avg + ymax
if frame_count % frame_average == 0:
frame_count = 0
xmin_avg = xmin_avg/frame_average
xmax_avg = xmax_avg/frame_average
ymin_avg = ymin_avg/frame_average
ymax_avg = ymax_avg/frame_average
print(xmin_avg, end=",")
print(ymin_avg, end=",")
print(xmax_avg, end=",")
print(ymax_avg, end="\n")
ymin_avg = ymin_avg + (ymax_avg - ymin_avg)/2. - H/2.
xmin_avg = xmin_avg + (xmax_avg - xmin_avg)/2. - W/2.
print("150: ",xmin_avg, end=",")
print("150: ",ymin_avg, end="\n")
gt = [xmin_avg, ymin_avg, W, H]
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
bboxes = np.zeros((1, 4))
#bboxes[0,:] = pos_x-target_w/2, pos_y-target_h/2, target_w, target_h
bboxes[0,:] = pos_x-W/2, pos_y-H/2, W, H
print(len(contours))
testing_counter = testing_counter + 1
print("Testing_counter: ",testing_counter)
show_frame(np.asarray(Image.open(frame)), gt, 1)
print("Cat is detected")
print("Starting the tracker ...")
if (bboxes[0,1] + bboxes[0,3]/2) < (Y + H/2 - 40):
print("Command: Raise the head")
angle = angle + 0.5
if angle > 44.5:
angle = 44.5
elif (bboxes[0,1] + bboxes[0,3]/2) > (Y + H/2 + 40):
print("Command: Lower the head")
angle = angle - 0.5
if angle < 0:
angle = 0
else:
pass
set_head_angle_action = robot.set_head_angle(degrees(angle), max_speed=20, in_parallel=True)
if straight(bboxes[0,:])[0] != 0 and turn(bboxes[0,:])[0] != 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0] + turn(bboxes[0,:])[0], straight(bboxes[0,:])[1] + turn(bboxes[0,:])[1])
detected_centroid = 0
elif straight(bboxes[0,:])[0] == 0 and turn(bboxes[0,:])[0] == 0:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
elif straight(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(turn(bboxes[0,:])[0], turn(bboxes[0,:])[1])
detected_centroid = 0
elif turn(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0], straight(bboxes[0,:])[1])
detected_centroid = 0
else:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
if detected_centroid > 20//frame_average:
detected_centroid = 0
print("Reached a stable state.........\t\t\t\t\t\t\t\t STABLE")
# Go near the object
set_head_angle_action.wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
robot.wait_for_all_actions_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
target_frame_count = 1
while True:
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame1 = latest_img.raw_image
target_frame1 = target_frame1.resize((640,480), Image.ANTIALIAS)
#target_frame1 = target_frame1.convert('L')
target_frame1 = np.asarray(target_frame1)
#orb1 = cv2.ORB_create(500)
#kp1 = orb1.detect(target_frame1,None)
#kp1, des1 = orb1.compute(target_frame1, kp1)
#features_img1 = cv2.drawKeypoints(target_frame1, kp1, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",features_img1)
plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",target_frame1)
drive_straight_action = robot.drive_straight(distance=cozmo.util.distance_mm(distance_mm=10),speed=cozmo.util.speed_mmps(10), in_parallel=True)
drive_straight_action.wait_for_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame2 = latest_img.raw_image
target_frame2 = target_frame2.resize((640,480), Image.ANTIALIAS)
#target_frame2 = target_frame2.convert('L')
target_frame2 = np.asarray(target_frame2)
#orb2 = cv2.ORB_create(500)
#kp2 = orb2.detect(target_frame2,None)
#kp2, des2 = orb2.compute(target_frame2, kp2)
#features_img2 = cv2.drawKeypoints(target_frame2, kp2, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",features_img2)
plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",target_frame2)
target_frame_count = target_frame_count + 1
'''
matcher = cv2.DescriptorMatcher_create(cv2.DESCRIPTOR_MATCHER_BRUTEFORCE_HAMMING)
matches = matcher.match(des1, des2, None)
matches.sort(key=lambda x: x.distance, reverse=False)
matches = matches[:10]
imMatches = cv2.drawMatches(target_frame1, kp1, target_frame2, kp2, matches, None)
cv2.imwrite("matches_tf1_tf2.jpg", imMatches)
points1 = np.zeros((len(matches), 2), dtype=np.float32)
points2 = np.zeros((len(matches), 2), dtype=np.float32)
for i, match in enumerate(matches):
points1[i, :] = kp1[match.queryIdx].pt
points2[i, :] = kp2[match.trainIdx].pt
print("Points1 [{}]: {}".format(i,points1[i][0]), points1[i][1],"\tPoints2: ",points2[i][0], points2[i][1])
index = None
dist1_x = []
dist2_x = []
for index in range(len(points1)):
dist1_x.append((W/2.)-points1[index][0]) # Extract only the x-coordinate
dist2_x.append((W/2.)-points2[index][0]) # Extract only the x-coordinate
fw_x = 1./((1./np.array(dist2_x)) - (1./np.array(dist1_x))) # Calculate the image plane to obj plane mapping in x direction
pt1_x = []
pt2_x = []
for index in range(len(points1)):
pt1_x.append(fw_x[index]/(W/2. - points1[index][0]))
pt2_x.append(fw_x[index]/(W/2. - points2[index][0]))
print("Approx. distance[{}]: {}".format(index, pt1_x[index]))
if len(pt2_x) < 10:
break
'''
sys.exit(0)
else: # Detected Dog
dog_counter += 1
if dog_counter >= 6: # Transition to Dog only if Dog appeared for more than 6 times
cat_counter = 0
current_state = 2
# Current State is Dog
elif current_state == 2:
print('\t\t\t\t\t\t\t\t\t\t\t\tDog')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
dog_counter = 0
elif next_state == 2: # Detected Dog itself
dog_counter +=1
if dog_counter >= 30:
print('Cozmo sees a Dog')
robot.drive_wheels(-50, -50)
time.sleep(3)
robot.drive_wheels(70, -70)
time.sleep(2.8)
robot.drive_wheels(0, 0)
break
else: # Detected Cat
cat_counter += 1
if cat_counter >= 6: # Transition to Cat only if Cat appeared for more than 6 times
dog_counter = 0
current_state = 1
def light_when_face(robot: cozmo.robot.Robot):
'''The core of the light_when_face program'''
# Move lift down and tilt the head up
robot.move_lift(-3)
robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
face = None
print("Press CTRL-C to quit")
while True:
if face and face.is_visible:
robot.set_all_backpack_lights(cozmo.lights.blue_light)
else:
robot.set_backpack_lights_off()
# Wait until we we can see another face
try:
face = robot.world.wait_for_observed_face(timeout=30)
if face.name.strip() != "":
robot.say_text("I see " + face.name).wait_for_completed()
else:
robot.say_text(
"I see a face. Who is this?").wait_for_completed()
except asyncio.TimeoutError:
print("Didn't find a face.")
return
time.sleep(.1)
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment description for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent, scale
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
while not stopevent.is_set():
grid.clearObstacles()
grid.setStart(convert_pose_to_coords(robot.pose))
# Find up to three light cubes using built-in functions
cubes = [
robot.world.get_light_cube(cozmo.objects.LightCube1Id),
robot.world.get_light_cube(cozmo.objects.LightCube2Id),
robot.world.get_light_cube(cozmo.objects.LightCube3Id)
]
desired_angle = cubes[0].pose.rotation.angle_z.degrees
# Set the location of the goal in the grid, or set it to the middle
if isFound(cubes[0]):
grid.clearGoals()
target_coords = convert_pose_to_coords(cubes[0].pose)
approach_x = approach_y = 0
grid.addObstacles(expand_obstacle(target_coords))
approach_angle = cubes[0].pose.rotation.angle_z
approach_x = 3 * round(math.cos(approach_angle.radians), 0)
approach_y = 3 * round(math.sin(approach_angle.radians), 0)
grid.addGoal(
(target_coords[0] - approach_x, target_coords[1] - approach_y))
else:
while not isFound(cubes[0]):
robot.turn_in_place(
cozmo.util.degrees(10)).wait_for_completed()
cubes[0] = robot.world.get_light_cube(
cozmo.objects.LightCube1Id)
if isFound(cubes[1]):
grid.addObstacles(
expand_obstacle(convert_pose_to_coords(cubes[1].pose)))
if isFound(cubes[2]):
grid.addObstacles(
expand_obstacle(convert_pose_to_coords(cubes[2].pose)))
# Run Astar
astar(grid, heuristic)
# Move to next
dx = (grid.getPath()[1][0] - grid.getPath()[0][0]) * scale
dy = (grid.getPath()[1][1] - grid.getPath()[0][1]) * scale
dh = cozmo.util.radians(math.atan2(dy,
dx)) - robot.pose.rotation.angle_z
robot.turn_in_place(dh).wait_for_completed()
robot.drive_straight(
cozmo.util.distance_mm(math.sqrt(dx * dx + dy * dy)),
cozmo.util.speed_mmps(25)).wait_for_completed()
if math.sqrt(
(robot.pose.position.x - cubes[0].pose.position.x)**2 +
(robot.pose.position.y - cubes[0].pose.position.y)**2) < 100:
robot.turn_in_place(
cozmo.util.degrees(
desired_angle -
robot.pose.rotation.angle_z.degrees)).wait_for_completed()
stopevent.set()
def move_to_next_side(robot: cozmo.robot.Robot):
robot.turn_in_place(degrees(-45)).wait_for_completed()
robot.drive_straight(distance_mm(400), speed_mmps(200), False, False,
0).wait_for_completed()
robot.turn_in_place(degrees(130)).wait_for_completed()
take_photos(robot)
def cozmo_program(robot: cozmo.robot.Robot):
robot.say_text("variation 4").wait_for_completed()
def move_straight(robot: cozmo.robot.Robot, unit):
robot.drive_straight(cozmo.util.distance_mm(unit), cozmo.util.speed_mmps(150)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
print("Cozmo SDK has behavior control...")
robot.world.image_annotator.add_annotator('robotState', RobotStateDisplay)
robot.enable_device_imu(True, True, True)
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
robot.move_lift(-1)
robot.say_text("Hello World!").wait_for_completed()
time.sleep(1)
robot.say_text("Let's take pictures!").wait_for_completed()
time.sleep(1)
# take pictures
# myargs = ["1", "order", "drone", "inspection"]
myargs = sys.argv[1:]
if len(myargs) <= 1:
sys.exit("Incorrect arguments")
num_images_per_type = int(
myargs[0]) # number of images to take of each type of object
print("Taking ", num_images_per_type, "images each of ", myargs[1:])
for type in myargs[1:]:
for i in range(num_images_per_type):
time.sleep(.5)
robot.say_text(type).wait_for_completed()
time.sleep(3)
latest_image = robot.world.latest_image
new_image = latest_image.raw_image
timestamp = datetime.datetime.now().strftime("%dT%H%M%S%f")
new_image.save("./outputs/" + str(type) + "_" + timestamp + ".bmp")
time.sleep(3)
time.sleep(0.5)
robot.say_text("I will drive now!").wait_for_completed()
time.sleep(0.5)
robot.drive_straight(distance_mm(1000),
speed_mmps(1000)).wait_for_completed()
robot.say_text("Check out my arms!").wait_for_completed()
robot.move_lift(1)
time.sleep(1)
robot.move_lift(-1)
time.sleep(1)
robot.say_text("Pick me up and shake me around!").wait_for_completed()
time.sleep(2)
robot.say_text("You can put me down now!").wait_for_completed()
f = open("./outputs/test_output.txt", 'r+')
f.seek(0)
f.write("Min Gyro: " + str(min(gyro)) + "\n")
f.write("Max Gyro: " + str(max(gyro)) + "\n")
f.write("Min Acc: " + str(min(acc)) + "\n")
f.write("Max Acc: " + str(max(acc)))
f.truncate()
f.close()
if (all(i > -10
for i in min(gyro)) and all(i < 10 for i in max(gyro))) and (all(
i > -25000 for i in min(acc)) and all(i < 25000
for i in max(acc))):
robot.say_text("Sensors look good!").wait_for_completed()
else:
robot.say_text("Sensors look bad!").wait_for_completed()
def cozmo_tap_game(robot: cozmo.robot.Robot):
"""
The main game
"""
#Setup
speed_tap_game = SpeedTapEngine(robot)
robot_game_action = CozmoPlayerActions()
#emotion manipulation
if robot_game_action.sad_not_angry:
reaction_bias = "sad"
else:
reaction_bias = "angry"
cozmo.logger.info(
"Starting tap game with reaction time %s and game reaction bias %s" %
(robot_game_action.reaction_time, reaction_bias))
cozmo.logger.info("Player %s : Cozmo " % COZMO_ID)
cozmo.logger.info("Player %s : Human" % PLAYER_ID)
# Robot selects cube
robot_cube, player_cube = speed_tap_game.cozmo_setup_game()
time.sleep(0.25)
# Participant select cube
monitor_player_tap = Human_Listener(robot, player_cube, speed_tap_game)
game_complete = False
winner = 0
score_to = 5
monitor_player_tap.game_on = True
monitor_player_tap.start()
deal_count = 1
try:
while not winner:
# robot wait for the light to change on cube i.e. light to be dealt
robot_game_action.act_out(robot, "wait")
deal_type = speed_tap_game.deal_hand()
cozmo.logger.info("Hand %s delt" % deal_count)
deal_count += 1
# Cozmo's tap decision
tapped = robot_game_action.cozmo_tap_decision(
robot, deal_type, speed_tap_game)
# Give player time to take decision
time.sleep(1)
# Current deal is now deactivated
speed_tap_game.deactivate_current_deal()
#Check who scored and do the right emotion display for winning/losing hand
if speed_tap_game.deal_score[-1] == PLAYER_ID:
if speed_tap_game.deal_score.count(PLAYER_ID) >= score_to:
winner = PLAYER_ID
robot_game_action.act_out(robot, "lose_hand")
elif speed_tap_game.deal_score[-1] == COZMO_ID:
if speed_tap_game.deal_score.count(COZMO_ID) >= score_to:
winner = COZMO_ID
robot_game_action.act_out(robot, "win_hand")
# stop light chaser and prep for next round
robot_cube.stop_light_chaser()
player_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_cube.set_lights_off()
cozmo.logger.info("Score Board : %s" % speed_tap_game.deal_score)
cozmo.logger.info("Cozmo : %s" %
speed_tap_game.deal_score.count(COZMO_ID))
cozmo.logger.info("Player : %s" %
speed_tap_game.deal_score.count(PLAYER_ID))
# clear up games to show final gameresult
robot_cube.stop_light_chaser()
player_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_cube.set_lights_off()
monitor_player_tap.game_on = False
robot_game_action.act_out(robot, "stand_back")
# Indicate win/loss to player
if winner == COZMO_ID:
robot_cube.set_lights(cozmo.lights.green_light.flash())
player_cube.set_lights(cozmo.lights.red_light.flash())
elif winner == PLAYER_ID:
player_cube.set_lights(cozmo.lights.green_light.flash())
robot_cube.set_lights(cozmo.lights.red_light.flash())
robot.go_to_object(robot_cube, distance_mm(60.0)).wait_for_completed()
cozmo.logger.info("Final Score Cozmo : %s" %
speed_tap_game.deal_score.count(COZMO_ID))
cozmo.logger.info("Final Score Player : %s" %
speed_tap_game.deal_score.count(PLAYER_ID))
# Game end emotion act display
if winner == COZMO_ID:
robot_game_action.act_out(robot, "win_game")
else:
robot_game_action.act_out(robot, "lose_game")
finally:
monitor_player_tap.game_on = False
robot_cube.stop_light_chaser()
player_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_cube.set_lights_off()
monitor_player_tap.join()
del speed_tap_game
del player_cube
del robot_cube
def cozmo_program(robot: cozmo.robot.Robot):
time.sleep(2) #go left and right
robot.drive_wheels(100,
-100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.3)
robot.drive_wheels(-100,
100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.6)
robot.drive_wheels(100,
-100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.3)
robot.move_head(1)
time.sleep(1)
robot.drive_wheels(100,
-100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.3)
robot.drive_wheels(-100,
100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.6)
robot.drive_wheels(100,
-100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.4)
time.sleep(1)
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabChatty).wait_for_completed() #chat
robot.play_anim_trigger(
cozmo.anim.Triggers.VC_Alrighty).wait_for_completed() #happy
time.sleep(2)
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment document for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
foundGoal = False
foudnCenter = False
setPath = False
while not stopevent.is_set():
if foundGoal == False:
# find goal
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
try:
cube = robot.world.wait_for_observed_light_cube(timeout=5)
print("Found cube: %s" % cube)
foundGoal = True
pos = cube.pose.position
x, y, z = pos.x, pos.y, pos.z
cube_angle = cube.pose.rotation.angle_z.degrees
print("cube angle is: ", cube_angle)
a, b = 0, 0
if cube_angle > -22.5 and cube_angle < 22.5:
a, b = -1, 0
if cube_angle >= 22.5 and cube_angle < 67.5:
a, b = -1, -1
if cube_angle >= 67.5 and cube_angle < 112.5:
a, b = 0, -1
if cube_angle >= 112.5 and cube_angle < 157.5:
a, b = 1, -1
if cube_angle >= 157.5 or cube_angle <= -157.5:
a, b = 1, 0
if cube_angle > -67.5 and cube_angle <= -22.5:
a, b = -1, 1
if cube_angle > -112.5 and cube_angle <= -67.5:
a, b = 0, 1
if cube_angle > -157.5 and cube_angle <= -112.5:
a, b = 1, 1
obs1 = int(x / grid.scale + grid.getStart()[0] +
0.5), int(y / grid.scale + grid.getStart()[1] + 0.5)
add_obs = []
for i in range(-1, 2, 1):
for j in range(-1, 2, 1):
ob = obs1[0] + i, obs1[1] + j
add_obs.append(ob)
grid.addObstacles(add_obs)
goal = obs1[0] + a * 2, obs1[1] + b * 2
print(goal)
grid.clearGoals()
grid.addGoal(goal)
except asyncio.TimeoutError:
print("Didn't find a cube")
goal = int(grid.width / 2), int(grid.height / 2)
grid.addGoal(goal)
foundGoal = True
foundCenter = True
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
else:
# astar
# astar(grid)
# get path
# path = grid.getPath() #a list of cells
robot_pose = robot.pose
print(robot_pose)
rx, ry = robot_pose.position.x, robot_pose.position.y
cx = int(rx / grid.scale + grid.getStart()[0] + 0.5)
cy = int(ry / grid.scale + grid.getStart()[1] + 0.5)
# update start
grid.clearStart()
grid.setStart((cx, cy))
astar(grid, heuristic)
path = grid.getPath()
print(path)
prev = path[0]
for p in path[1:]:
diff = p[0] - prev[0], p[1] - prev[1]
# Needs to verify
curr_angle = robot.pose.rotation.angle_z
angle = getAngle(diff)
angle_to_turn = angle - curr_angle.degrees
robot.turn_in_place(
degrees(angle_to_turn)).wait_for_completed()
d = math.sqrt(math.pow(diff[0], 2) + math.pow(diff[1], 2))
robot.drive_straight(distance_mm(d * 50),
speed_mmps(40)).wait_for_completed()
posX, posY = grid.scale * (p[0] - 1), grid.scale * (p[1] - 1)
# posX, posY = 25 * p[0] - rx, 25 * p[1] - ry
# pose = cozmo.util.Pose(posX, posY, 0.0, q0 = 0.0, q1 = 0.0, q2 = 0.0, q3 = 0.0)
# Everytime we move, check to see if there's an obstacle in our vision
# pose = cozmo.util.Pose(posX, posY, 0.0, angle_z = cozmo.util.degrees(angle))
# robot.go_to_pose(pose, in_parallel = True).wait_for_completed()
# robot.go_to_pose(pose)
# time.sleep(0.1)
print(p)
print(robot.pose.position.x, robot.pose.position.y)
print(posX, posY)
prev = p
# ca = -1.0*cube_angle
# pose = cozmo.util.Pose(robot.pose.position.x, robot.pose.position.y, robot.pose.position.z, angle_z = degrees(ca))
# robot.go_to_pose(pose).wait_for_completed()
if foundCenter == True: #not in the goal yet
foundGoal = False
foundCenter = False
else:
curr_angle = robot.pose.rotation.angle_z
angle = cube_angle
angle_to_turn = angle - curr_angle.degrees
robot.turn_in_place(
degrees(angle_to_turn)).wait_for_completed()
break # Your code here
def cozmo_program(robot: cozmo.robot.Robot):
# Make sure Cozmo's head and arm are at reasonable levels
robot.set_head_angle(degrees(10.0)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.say_text(
f"I'm going to take photos of {sys.argv[1]}").wait_for_completed()
# Set directory to the Category that Cozmo is going to photograph
global directory
directory = sys.argv[1]
if not os.path.exists('data'):
os.makedirs('data')
if not os.path.exists(f'data/{directory}'):
os.makedirs(f'data/{directory}')
# Anytime Cozmo sees a "new" image, take a photo
robot.add_event_handler(cozmo.world.EvtNewCameraImage, on_new_camera_image)
# Initial photo sesh
robot.drive_straight(distance_mm(-200), speed_mmps(100), False, False,
0).wait_for_completed()
take_photos(robot)
# Get all the angles
#for i in range(3):
# move_to_next_side(robot)
# And we're done here
robot.say_text("All done!").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin).wait_for_completed()
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment description for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
global found_cubes
found_cubes = []
goal_degrees = []
grid.addGoal((grid.width / 2, grid.height / 2))
found_goal = False
astar(grid, heuristic)
path = grid.getPath()
path_index = 0
grid_init_start_pose = grid.getStart()
(robot_grid_x, robot_grid_y) = grid.getStart()
robot.set_head_angle(degrees(0)).wait_for_completed()
robot.set_lift_height(1).wait_for_completed()
robot.say_text('Game is on').wait_for_completed()
while not stopevent.is_set():
new_cube = search_cube(robot, grid)
if not new_cube == None:
grid.clearStart()
grid.clearVisited()
grid.clearPath()
grid.setStart((robot_grid_x, robot_grid_y))
add_obstacle(
grid, new_cube, grid_init_start_pose
) # Add the obstacle for all cubes that had been found
if new_cube.cube_id == LightCube1Id:
new_cube.set_lights(cozmo.lights.blue_light)
goal_degrees = set_goal(
grid, new_cube, grid_init_start_pose
) # Update the goal coordinate while found cube 1
robot.say_text("It's the Goal").wait_for_completed()
found_goal = True
else:
new_cube.set_lights(cozmo.lights.red_light)
robot.say_text("It's an Obstacle").wait_for_completed()
# Replanning the path for
robot.say_text('Replanning').wait_for_completed()
try:
astar(grid, heuristic)
except:
robot.say_text("Cannot go to that place").wait_for_completed()
return
path_index = 0
path = grid.getPath()
path_index += 1
if path_index == len(path): # At the goal position
if not found_goal: # At the center of grid
path_index -= 1
robot.turn_in_place(Angle(degrees=-30)).wait_for_completed()
continue
else: # Arrived the final place
goal_degree = goal_degrees[f"{(robot_grid_x, robot_grid_y)}"]
robot.turn_in_place(
Angle(degrees=normalize_angle(
goal_degree - robot.pose.rotation.angle_z.degrees))
).wait_for_completed()
robot.say_text('Arrived').wait_for_completed()
break
current_pose = path[path_index - 1]
next_pose = path[path_index]
robot_grid_x += int(next_pose[0] - current_pose[0])
robot_grid_y += int(next_pose[1] - current_pose[1])
x = (next_pose[0] - current_pose[0]) * grid.scale
y = (next_pose[1] - current_pose[1]) * grid.scale
degree = ((90 * y / abs(y)) if x == 0 else math.degrees(
math.atan2(y, x))) - robot.pose.rotation.angle_z.degrees
# robot.turn_in_place(Angle(degrees=normalize_angle(degree))).wait_for_completed()
# robot.drive_straight(distance_mm(math.sqrt(x**2 + y**2)), speed_mmps(50), should_play_anim=False).wait_for_completed()
(x, y) = world_to_related(x, y, robot.pose.rotation.angle_z.degrees)
robot.go_to_pose(Pose(x, y, 0, angle_z=Angle(degrees=degree)),
relative_to_robot=True).wait_for_completed()
stopevent.wait()
def cozmo_say(robot: cozmo.robot.Robot):
if (msg is not None) and (len(msg) > 0):
robot.say_text(msg).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
time.sleep(2)
robot.move_lift(50)
robot.drive_straight(distance_mm(-50),
speed_mmps(150)).wait_for_completed() #drive forward
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabVampire).wait_for_completed() #grossed out
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabSquint1).wait_for_completed() #squint
robot.move_lift(-50) #put arms down
robot.drive_wheels(-1000,
1000,
l_wheel_acc=-250,
r_wheel_acc=250,
duration=1) #drive away
time.sleep(.5)
robot.drive_straight(distance_mm(200),
speed_mmps(150)).wait_for_completed()
def do_turn(robot: cozmo.robot.Robot):
drive_angle = extract_float(turn)
if drive_angle is not None:
robot.turn_in_place(degrees(drive_angle)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
global isTakingPicture
global targetObject
targetObject = sys.argv[1]
if os.path.exists('photos'):
shutil.rmtree('photos')
if not os.path.exists('photos'):
os.makedirs('photos')
robot.say_text(
f"Somebody lost the {targetObject}. Don't worry, I'll find it."
).wait_for_completed()
# reset Cozmo's arms and head
robot.set_head_angle(degrees(10.0)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.add_event_handler(cozmo.world.EvtNewCameraImage, on_new_camera_image)
while not discoveredObject:
isTakingPicture = False
robot.turn_in_place(degrees(45)).wait_for_completed()
isTakingPicture = True
time.sleep(2)
isTakingPicture = False
if discoveredObject:
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
robot.say_text(
f"Oh yay! I've found the {targetObject}").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.MajorWin).wait_for_completed()
async def run(robot: cozmo.robot.Robot):
#disable all annotations from the interface
robot.world.image_annotator.annotation_enabled = True
#add your own annotation called 'box'
robot.world.image_annotator.add_annotator('box', BoxAnnotator)
#make sure the camera receives image data (enable the camera)
robot.camera.image_stream_enabled = True
#make sure you receive color images from the robot
robot.camera.color_image_enabled = True
#enabling auto_exposure to constantly update the exposure time and gain values on recent images
robot.camera.enable_auto_exposure = True
#current camera gain setting, current camera exposure in ms, and...?
gain, exposure, mode = 390, 3, 1
try:
while True:
#current event is the new raw image from the camera that lasts 30ms
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage,
timeout=30) #get camera image
#if we got an image (of course we will)
if event.image is not None:
#convert the image into into an array and change the color scheme
image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_BGR2RGB)
#make sure we refresh the image
if mode == 1:
robot.camera.enable_auto_exposure = True
else:
#force the specified exposure time and gain values to the camera
robot.camera.set_manual_exposure(exposure, fixed_gain)
#find the cube (using the current image, and the lower and upper bounds of 'yellow'
cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER)
#print the coords in the terminal or None if no cube is detected
print(cube)
#set the returned cube value to BoxAnnotator's cube
BoxAnnotator.cube = cube
################################################################
# Todo: Add Motion Here
################################################################
#Want to center the cube in cozmo's vision, move so it is close to the center of the image
#Once cozmo's camera is centered on the cube, move forward.
#if cozmo doesn't see the cube, rotate 45 degrees
if BoxAnnotator.cube is None:
await robot.turn_in_place(degrees(45)).wait_for_completed()
if BoxAnnotator.cube is not None:
#check to see if we are close enough to the cube
if (BoxAnnotator.cube[2] >
90): #was 90 for quarter of the screen
print("I am close to the object")
robot.stop_all_motors()
#cozmo detects cube on the left of the screen, reorient itself
elif (BoxAnnotator.cube[0] < 103):
print(
"Cube on left half of screen, reorienting myself")
#orient yourself so cube is near the center of the screen
await robot.turn_in_place(degrees(15)
).wait_for_completed()
#cozmo detects cube on right half of screen, reorient itself
elif (BoxAnnotator.cube[0] > 206):
print(
"Cube on right half of screen, reorienting myself")
#orient yourself so cube is in center of the screen
await robot.turn_in_place(degrees(-15)
).wait_for_completed()
#cube is centered, move towards it
elif (BoxAnnotator.cube[0] > 103
and BoxAnnotator.cube[0] < 206):
#cube is in center of screen, move forward
print(
"Cube is near the center of the screen, moving forward"
)
await robot.drive_straight(
distance_mm(25.4),
speed_mmps(25.4),
in_parallel=True).wait_for_completed()
#We can also use cozmo.go_to_object()?
#no? CUstomObject instances are not supported
#any keyboard interrupt will exit the program
except KeyboardInterrupt:
print("")
print("Exit requested by user")
#if cozmo is doing something, let him finish before doing the next task
except cozmo.RobotBusy as e:
print(e)
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
###################
reqConfidentFrames = 10
reqUnconfidentFrames = 10
############YOUR CODE HERE#################
condition = True
estimated = [0,0,0,False]
ballObj = Robot(0, 0, 0)
STATE = RobotStates.LOCALIZING
trueVal = 0
falseVal = 0
dist_to_ball = None
robotLen = 1
while (condition):
curr_pose = robot.pose
img = image_processing(robot)
(markers, ball) = cvt_2Dmarker_measurements(img)
dist_to_ball = calcDistance(ball)
if(dist_to_ball is not None and estimated[3]):
ballx = estimated[0] + math.cos(estimated[2]) * dist_to_ball
bally = estimated[1] + math.sin(estimated[2]) * dist_to_ball
ballObj.set_pos(ballx, bally)
#thinkg about false positives
#go to pose calculation
#How to make a pose??
#rotation plus position
#once localized we can tell what directino we are looking in and what position we are in
#add to cozmo's pose robot.Pose.pose.position in the direction of robot.Pose.pose.rotation
odom = compute_odometry(curr_pose)
estimated = pf.update(odom, markers)
gui.show_particles(pf.particles)
gui.show_robot(ballObj)
gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3])
gui.updated.set()
if STATE == RobotStates.LOCALIZING:
robot.drive_wheels(-5, -5)
if estimated[3]:
trueVal += 1
else:
trueVal = 0
if trueVal > reqConfidentFrames && dist_to_ball is not None:
robot.stop_all_motors()
STATE = RobotStates.TRAVELING
if STATE == RobotStates.TRAVELING:
h = math.degrees(math.atan2(goal[1] - estimated[1], goal[0] - estimated[0]))
dx = ballObj.x - estimated[0] - (math.cos(h) * robotLen)
dy = ballObj.y - estimated[1] - (math.sin(h) * robotLen)
dh = h - estimated[2]
our_go_to_pose(robot, curr_pose, dx, dy, dh)
if not estimated[3]:
falseVal += 1
else:
falseVal = 0
if trueVal > reqUnconfidentFrames:
robot.stop_all_motors()
STATE = RobotStates.LOCALIZING
STATE = RobotStates.SHOOT
if STATE == RobotStates.SHOOT:
pass
STATE == RobotStates.RESET
if STATE == RobotStates.RESET:
pass
STATE = RobotStates.TRAVELING
last_pose = curr_pose
def excited(robot: cozmo.robot.Robot):
trigger = cozmo.anim.Triggers.CodeLabExcited
play_animation(robot,trigger)
robot.turn_in_place(degrees(-130)).wait_for_completed()
def cozmo_rigth_turn(robot: cozmo.robot.Robot):
global DONKEY_COZMO_TURN_ANGLE
robot.turn_in_place(degrees(-1.0 * DONKEY_COZMO_TURN_ANGLE)).wait_for_completed()
def move_backward(robot: cozmo.robot.Robot):
# Drive forwards for 150 millimeters at 50 millimeters-per-second.
robot.drive_straight(distance_mm(-100), speed_mmps(50)).wait_for_completed()
#cozmo.run_program(move_backward)
async def robot_main(robot: cozmo.robot.Robot):
img_clf = ImageClassifier()
(train_raw, train_labels) = img_clf.load_data_from_folder('./train/')
train_data = img_clf.extract_image_features(train_raw)
img_clf.train_classifier(train_data, train_labels)
robot.move_lift(-3)
await robot.set_head_angle(cozmo.util.degrees(10)).wait_for_completed()
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
images = []
while True:
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage,
timeout=30)
#convert camera image to opencv format
image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY)
data = img_clf.extract_image_features([image])
labels = img_clf.predict_labels(data)
images.append(labels[0])
print(labels[0])
if len(images
) < 3 or images[-1] != images[-2] or images[-2] != images[-3]:
continue
if labels[0] == 'none':
continue
await robot.say_text(labels[0],
use_cozmo_voice=True,
in_parallel=True,
num_retries=5).wait_for_completed()
if labels[0] == 'order':
pass
elif labels[0] == 'drone':
pass
elif labels[0] == 'inspection':
await robot.drive_wheels(25, -25)
time.sleep(1)
await robot.drive_wheels(-25, 25)
time.sleep(2)
await robot.drive_wheels(25, -25)
time.sleep(1)
await robot.drive_wheels(0, 0)
elif labels[0] == 'truck':
await robot.drive_wheels(25, 25)
time.sleep(2)
await robot.drive_wheels(-25, -25)
time.sleep(2)
await robot.drive_wheels(0, 0)
elif labels[0] == 'hands':
pass
elif labels[0] == 'place':
robot.move_lift(3)
time.sleep(.25)
robot.move_lift(-3)
time.sleep(.25)
elif labels[0] == 'plane':
pass
time.sleep(.25)
async def main(robot: cozmo.robot.Robot):
#initialize the machine state
state_machine = CozmoStates()
state_machine.set_cozmo_coords(robot.pose.position)
#event handlers
robot.add_event_handler(cozmo.objects.EvtObjectAppeared,
state_machine.handle_object_appeared)
robot.add_event_handler(cozmo.objects.EvtObjectDisappeared,
state_machine.handle_object_disappeared)
#are we looking for cube 2?
look_for_cube_2 = False
#cube1 definition
await robot.world.define_custom_cube(
CustomObjectTypes.CustomType00,
state_machine.cube_arr[state_machine.current_cube], 44, 30, 30, False)
while True:
print('######looping#####')
#get camera image
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage,
timeout=30)
#refresh the coords
state_machine.set_cozmo_coords(robot.pose)
#triggers when we want the first cube
if state_machine.current_cube == 1:
print('cube = 1')
if state_machine.cube_coords == False:
state_machine.cube_not_found()
else:
state_machine.calc_goto_coords()
if (state_machine.calc_dist_from_cozmo() >
state_machine.proximity_distance):
if state_machine.goto_y >= 20:
state_machine.found_cube_left()
elif state_machine.goto_y < -20:
state_machine.found_cube_right()
else:
print('Cube ahead!')
state_machine.found_cube_ahead()
else:
robot.stop_all_motors()
state_machine.on_enter_near_cube()
#triggers when we want cube 2
if state_machine.current_cube == 2:
print('cube = 2')
#reset the data if this is the first instance
if look_for_cube_2 == False:
robot.stop_all_motors()
print('\nPART 1 DONE! GO TO CUBE 2\n')
await robot.say_text('Look for cube 2',
in_parallel=True).wait_for_completed()
state_machine.set_cozmo_coords(robot.pose)
state_machine.calc_goto_coords()
print(state_machine.calc_dist_from_cozmo())
look_for_cube_2 = True
state_machine.cube_not_found()
state_machine.cube_coords = False
#define the second cube
await robot.world.define_custom_cube(
CustomObjectTypes.CustomType00,
state_machine.cube_arr[state_machine.current_cube], 44, 30,
30, False)
#where does cozmo need to move?
if (state_machine.cube_coords == False) or (state_machine.see_cube
== False):
if state_machine.get_current_state() == 'drive_straight':
if state_machine.goto_y < state_machine.mid_screen:
state_machine.found_cube_left()
elif state_machine.goto_y < -20:
state_machine.found_cube_right()
else:
print('Cube ahead!')
state_machine.found_cube_ahead()
else:
state_machine.cube_not_found()
#cozmo sees cube
else:
state_machine.calc_goto_coords()
if (state_machine.calc_dist_from_cozmo() >
state_machine.proximity_distance):
if state_machine.goto_y < -20:
state_machine.found_cube_right()
elif state_machine.goto_y > 20:
state_machine.found_cube_left()
else:
state_machine.found_cube_ahead()
else:
robot.stop_all_motors()
state_machine.on_enter_near_cube()
#update states
last_state = state_machine.get_last_state()
current_state = state_machine.get_current_state()
#was there a state transition? WIll only trigger if a cube was detected
if last_state is not current_state:
#audio cue, takes a long time, need to comment out
#await robot.say_text('b', in_parallel = True).wait_for_completed()
#print the state transition to terminal & reset the state
print('Last state: ' + last_state)
print('Current state: ' + current_state)
print('Coords to cube: ')
#some reason, cant print this with the Coords to cube: print statement
print(state_machine.calc_goto_coords())
#update the state
state_machine.set_last_state(current_state)
# the anki victor text stuff, taken and modified
#
faceImage = Image.new('RGBA', (128, 64), (0, 0, 0, 255))
dc = ImageDraw.Draw(faceImage)
try:
font_file = ImageFont.truetype('arial.ttf', 20)
except IOError:
print('IoError')
try:
font_file = ImageFont.truetype(
'/usr/share/fonts/noto/NotoSans-Medium.ttf', 20)
except IOError:
print('IoError 2')
pass
dc.text((0, 0),
current_state,
fill=(255, 255, 255, 255),
font=font_file)
screen_data = cozmo.oled_face.convert_image_to_screen_data(
faceImage)
robot.display_oled_face_image(screen_data,
600000000.0,
in_parallel=True)
#update how robot should move
await robot.drive_wheels(state_machine.get_l_motor_speed(),
state_machine.get_r_motor_speed())
def do_lift(robot: cozmo.robot.Robot):
lift_height = extract_float(lift)
if lift_height is not None:
robot.set_lift_height(height=lift_height).wait_for_completed()
def light(robot: cozmo.robot.Robot):
cube1 = robot.world.get_light_cube(LightCube1Id)
cube2 = robot.world.get_light_cube(LightCube2Id)
cube3 = robot.world.get_light_cube(LightCube3Id)
for i in range(10):
robot.set_all_backpack_lights(cozmo.lights.blue_light)
cube1.set_light_corners(cozmo.lights.green_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light)
cube2.set_light_corners(cozmo.lights.green_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light)
cube3.set_light_corners(cozmo.lights.green_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light)
time.sleep(.1)
robot.set_all_backpack_lights(cozmo.lights.green_light)
cube1.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.green_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light)
cube2.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.green_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light)
cube3.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.green_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light)
time.sleep(.1)
robot.set_all_backpack_lights(cozmo.lights.blue_light)
cube1.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.green_light,
cozmo.lights.blue_light)
cube2.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.green_light,
cozmo.lights.blue_light)
cube3.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.green_light,
cozmo.lights.blue_light)
time.sleep(.1)
robot.set_all_backpack_lights(cozmo.lights.green_light)
cube1.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.green_light)
cube2.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.green_light)
cube3.set_light_corners(cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.blue_light,
cozmo.lights.green_light)
time.sleep(.1)
def hesitate_long(robot: cozmo.robot.Robot):
trigger = cozmo.anim.Triggers.MeetCozmoFirstEnrollmentSayName #PatternGuessThinking #CodeLabThinking
play_animation(robot,trigger)
robot.turn_in_place(degrees(10)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): robot.drive_straight(distance_mm(5), speed_mmps(10)).wait_for_completed()
def propose_guess(robot: cozmo.robot.Robot, guess = ''):
robot.say_text(guess, voice_pitch = 0.5,play_excited_animation=False).wait_for_completed()
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
state = State()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
trigger = True
isBallFound = False
isRobotAtBall = False
left = False
right = False
robot.set_head_angle(radians(-0.23), in_parallel=True)
# prevPos = None
# direction = None
while trigger:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
h, w = opencv_image.shape
# print(w)
#find the ball
ball = find_ball.find_ball(opencv_image)
distance = calcDistance(ball)
# if prevPos
#set annotator ball
BallAnnotator.ball = ball
BallAnnotator.distance = distance
# BallAnnotator.direction = direction
if state.isCurState("START"):
robot.set_lift_height(0, in_parallel=True)
# robot.display_oled_face_image(image(state.cur), 10.0, in_parallel=True)
#spin around and search for ball
#Make a sound and print something on screen
# display for 1 second
if ball is None:
await robot.drive_wheels(17, -17)
else:
await robot.drive_wheels(0, 0, 0.5)
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
if state.isCurState("TRAVELING"):
# robot.display_oled_face_image(image(state.cur), 10.0, in_parallel=True)
#Print and sound off
# move towards ball
if distance is None:
if left:
lspeed = 5
rspeed = 2 * base
if right:
lspeed = 2 * base
rspeed = 5
await robot.drive_wheels(lspeed, rspeed)
if distance is not None:
if distance > 85:
base = 25
adj = (ball[0] - (w / 2)) / (distance**0.5)
# print(distance)
# print("adj:", adj)
left = adj < -0.75
right = adj > 0.75
if left:
lspeed = base
rspeed = base - adj
# print("LEFT")
elif right:
lspeed = base + adj
rspeed = base
# print("RIGHT")
else:
lspeed = base + 20
rspeed = base + 20
await robot.drive_wheels(lspeed, rspeed)
else:
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
if state.isCurState("END"):
# robot.display_oled_face_image(image(state.cur), 10.0, in_parallel = True)
#tap ball
#Screen and sound off
robot.set_lift_height(1, in_parallel=True).wait_for_completed()
robot.set_lift_height(0, in_parallel=True).wait_for_completed()
if distance is None:
if left or right:
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
elif distance > 85:
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
if state.isCurState("PAUSE"):
#pause for a moment
await robot.drive_wheels(0, 0, 0.05)
state.next()
robot.display_oled_face_image(image(state.cur),
100,
in_parallel=True)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
