def drive_to_charger(robot):
'''The core of the drive_to_charger program'''
# If the robot was on the charger, drive them forward and clear of the charger
if robot.is_on_charger:
# drive off the charger
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
# Start moving the lift down
robot.move_lift(-3)
# turn around to look at the charger
robot.turn_in_place(degrees(180)).wait_for_completed()
# Tilt the head to be level
robot.set_head_angle(degrees(0)).wait_for_completed()
# wait half a second to ensure Cozmo has seen the charger
time.sleep(0.5)
# drive backwards away from the charger
robot.drive_straight(distance_mm(-60), speed_mmps(50)).wait_for_completed()
# try to find the charger
charger = None
# see if Cozmo already knows where the charger is
if robot.world.charger:
if robot.world.charger.pose.origin_id == robot.pose.origin_id:
print("Cozmo already knows where the charger is!")
charger = robot.world.charger
else:
# Cozmo knows about the charger, but the pose is not based on the
# same origin as the robot (e.g. the robot was moved since seeing
# the charger) so try to look for the charger first
pass
if not charger:
# Tell Cozmo to look around for the charger
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=30)
print("Found charger: %s" % charger)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if charger:
# Attempt to drive near to the charger, and then stop.
action = robot.go_to_object(charger, distance_mm(65.0))
action.wait_for_completed()
print("Completed action: result = %s" % action)
print("Done.")
def transport_cube_I(cube: cozmo.objects.LightCube):
global robot,SIDE
# First cube was picked, place it next to the charger
charger = go_to_charger()
final_adjust(charger,60)
robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed()
putDown_cube(cube)
switch_cube_off(cube)
# Get back in front of charger
robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed()
robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed()
robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed()
return
def run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_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):
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()
def cozmo_program(cozmo_robot: cozmo.robot.Robot):
global robot
init_robot(cozmo_robot)
# Get off charger if on it
if(robot.is_on_charger):
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(110),speed_mmps(80)).wait_for_completed()
execute_procedure()
return
def transport_cube_III(cube: cozmo.objects.LightCube):
global robot,SIDE
# Third cube was picked, go place it next to the others
charger = go_to_charger()
final_adjust(charger,80)
robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed()
putDown_cube(cube)
switch_cube_off(cube)
# Get back in front of charger
robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed()
robot.turn_in_place(degrees(-SIDE*60)).wait_for_completed()
return
def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False):
# Final adjustement to properly face the charger.
# The position can be adjusted several times if
# the precision is critical, i.e. when climbing
# back onto the charger.
global robot,PI
while(True):
# Calculate positions
r_coord = [0,0,0]
c_coord = [0,0,0]
# Coordonates of robot and charger
r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise
r_coord[1] = robot.pose.position.y
r_coord[2] = robot.pose.position.z
r_zRot = robot.pose_angle.radians # .degrees or .radians
c_coord[0] = charger.pose.position.x
c_coord[1] = charger.pose.position.y
c_coord[2] = charger.pose.position.z
c_zRot = charger.pose.rotation.angle_z.radians
# Create target position
# dist_charger in mm, distance if front of charger
c_coord[0] -= dist_charger*math.cos(c_zRot)
c_coord[1] -= dist_charger*math.sin(c_zRot)
# Direction and distance to target position (in front of charger)
distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2)
vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]]
# Angle of vector going from robot's origin to target's position
theta_t = math.atan2(vect[1],vect[0])
print('CHECK: Adjusting position')
# Face the target position
angle = clip_angle((theta_t-r_zRot))
robot.turn_in_place(radians(angle)).wait_for_completed()
# Drive toward the target position
robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed()
# Face the charger
angle = clip_angle((c_zRot-theta_t))
robot.turn_in_place(radians(angle)).wait_for_completed()
# In case the robot does not need to climb onto the charger
if not critical:
break
elif(check_tol(charger,dist_charger)):
print('CHECK: Robot aligned relativ to the charger.')
break
return
def run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
robot.set_all_backpack_lights(cozmo.lights.red_light)
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession).wait_for_completed()
# make cozmo reverse 100mm (negative distance == reverse)
robot.drive_straight(distance=distance_mm(-100),
speed=speed_mmps(50)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.ReactToCliff).wait_for_completed()
robot.say_text("Hello").wait_for_completed()
async def execute_directions(robot, directions):
await robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
await robot.drive_straight(distance=distance_mm(directions.position.x * grid.scale), speed=speed_mmps(80)).wait_for_completed()
await robot.turn_in_place(angle=cozmo.util.Angle(degrees=90)).wait_for_completed()
await robot.drive_straight(distance=distance_mm(directions.position.y * grid.scale), speed=speed_mmps(80)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
# Les variables nécessaires
min_number = 1
max_number = 100
number = 0
turn = 0
# Définir les couleurs
light_yellow = Light(Color(name='yellow', rgb=(255, 255, 0)))
light_green = Light(Color(name='green', rgb=(0, 255, 0)))
light_blue = Light(Color(name='blue', rgb=(0, 0, 255)))
# Tourner jusqu'a détecter un visage
lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.FindFaces)
face = robot.world.wait_for_observed_face()
lookaround.stop()
# L'hummain trouvé est il connu ou inconnue
robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
if (face.name != ""):
robot.play_anim_trigger(
cozmo.anim.Triggers.AcknowledgeFaceNamed).wait_for_completed()
robot.say_text(face.name).wait_for_completed()
else:
robot.play_anim_trigger(
cozmo.anim.Triggers.AcknowledgeFaceUnnamed).wait_for_completed()
robot.say_text("Bonjour").wait_for_completed()
# Donner les explications du jeu
text = "Je vais deviner un nombre entre " + str(min_number) + " et " + str(
max_number) + "."
robot.say_text(text).wait_for_completed()
robot.say_text("Bleu si plus grand.").wait_for_completed()
robot.world.get_light_cube(LightCube1Id).set_lights(light_blue)
robot.say_text("Jaune si plus petit.").wait_for_completed()
robot.world.get_light_cube(LightCube2Id).set_lights(light_yellow)
robot.say_text("Vert si j'ai trouvé.").wait_for_completed()
robot.world.get_light_cube(LightCube3Id).set_lights(light_green)
# Jouer l'animation Cozmo content
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabHappy).wait_for_completed()
robot.say_text("Vert quand tu es prêt.").wait_for_completed()
# Attendre jusqu'a ce qu'un cube soit touché
ready = False
while ready is False:
target = robot.world.wait_for(cozmo.objects.EvtObjectTapped)
cube = robot.world.get_light_cube(target.obj.cube_id)
ready = cube.cube_id is LightCube3Id
# Générer un nombre aléatoire entre min_number et max_number
number = random.randint(min_number, max_number)
found = False
cheated = False
# Boucle de jeu
while found is False and cheated is False:
turn += 1
robot.drive_straight(distance_mm(-30),
speed_mmps(50)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabThinking).wait_for_completed()
robot.say_text(str(number)).wait_for_completed()
target = robot.world.wait_for(cozmo.objects.EvtObjectTapped)
cube = robot.world.get_light_cube(target.obj.cube_id)
# jouer l'animation suivat le cube touché
if cube.cube_id is LightCube3Id:
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabHappy).wait_for_completed()
found = True
elif cube.cube_id is LightCube1Id:
robot.say_text("Plus grand ?").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabUnhappy).wait_for_completed()
min_number = number + 1
elif cube.cube_id is LightCube2Id:
robot.say_text("Plus petit ?").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabUnhappy).wait_for_completed()
max_number = number - 1
else:
cheated = True
# Le joueur a t'il triché
if max_number < min_number:
cheated = True
# Tenter de générer un nouveau nombre
elif found is False:
number = round((min_number + max_number) / 2)
# Si COZMO à trouvé
if found is True:
text = str(turn) + " essais."
robot.say_text(text).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabWin).wait_for_completed()
else:
robot.say_text("Tu as triché").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabFrustrated).wait_for_completed()
def cozmo_idle(robot: cozmo.robot.Robot):
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
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.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
while True:
time.sleep(1.0)
latest_im = robot.world.latest_image
new_im = np.asarray(latest_im.raw_image, dtype="uint8")
f = feature.hog(new_im,
orientations=10,
pixels_per_cell=(48, 48),
cells_per_block=(4, 4),
feature_vector=True,
block_norm='L2-Hys')
type = img_clf.predict_labels([f])[10]
if type == "drone":
robot.say_text(type).wait_for_completed()
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
try:
cube = robot.world.wait_for_observed_light_cube(timeout=20)
except asyncio.TimeoutError:
print("There is no cube")
finally:
look_around.stop()
if cube is not None:
robot.go_to_object(cube, distance_mm(40)).wait_for_completed()
robot.pickup_object(cube, num_retries=2).wait_for_completed()
robot.drive_straight(distance_mm(100),
speed_mmps(55)).wait_for_completed()
robot.place_object_on_ground_here(cube).wait_for_completed()
robot.drive_straight(distance_mm(-100),
speed_mmps(55)).wait_for_completed()
elif type == "order":
robot.say_text(type).wait_for_completed()
robot.drive_wheels(50, 23.5, duration=21.55)
elif type == "inspection":
robot.say_text(type).wait_for_completed()
for times in range(4):
robot.set_lift_height(1, duration=2, in_parallel=True)
robot.drive_straight(distance_mm(200),
speed_mmps(50),
in_parallel=True).wait_for_completed()
robot.set_lift_height(0, duration=2,
in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
elif type == "plane":
robot.say_text(type).wait_for_completed()
elif type == "truck":
robot.say_text(type).wait_for_completed()
elif type == "hands":
robot.say_text(type).wait_for_completed()
elif type == "place":
robot.say_text(type).wait_for_completed()
time.sleep(1)
def en_charger(self, robot:cozmo.robot.Robot = None, cmd_args = None):
usage = "Cozmo tries to park on is charger, in 3 tries."
if robot is None:
return usage
trial = 1
# try to find the charger
charger = None
# see if Cozmo already knows where the charger is
if robot.world.charger:
if robot.world.charger.pose.origin_id == robot.pose.origin_id:
if log:
print("Cozmo already knows where the charger is!")
charger = robot.world.charger
else:
# Cozmo knows about the charger, but the pose is not based on the
# same origin as the robot (e.g. the robot was moved since seeing
# the charger) so try to look for the charger first
pass
if not charger:
# Tell Cozmo to look around for the charger
if log:
print("looking for the charger now...")
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=60)
print("Found charger: %s" % charger)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if charger:
if log:
print("lifting my arms to manouver...")
robot.move_lift(10)
# Attempt to drive near to the charger, and then stop.
if log:
print("Trial number %s" % trial)
print("Going for the charger!!!")
action = robot.go_to_pose(charger.pose)
action.wait_for_completed()
if log:
print("Completed action: result = %s" % action)
robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed()
if log:
print("Done.")
# Turn 180 (and 5) degrees, then goes backwards at full speed
if log:
print("Now the grand finalle: turn around and park!")
print("Turning...")
#this value needs to be tweaked (90 or 95)
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.turn_in_place(degrees(95)).wait_for_completed()
time.sleep( 1 )
if log:
print("Get out of the way: here I go!")
robot.drive_straight(distance_mm(-130), speed_mmps(150)).wait_for_completed()
if log:
print("checking if I did it...")
if robot.is_on_charger:
robot.move_lift(-8)
print("I did it! Yay!")
else:
print("I did not manage to dock in the charger =(")
print("Trying again...")
robot.world.charger = None
if log:
print("let me go a little bit further to be easier to see...")
robot.drive_straight(distance_mm(90), speed_mmps(50)).wait_for_completed()
trial += 1
if trial < 4:
self.en_charger(robot)
else:
print("tired of trying. Giving up =(")
def knock_cubes_over():
# This function allows to detect if the three cubes are stacked on each others.
# If it is the case, Cozmo will try to make them fall.
global robot
cube1 = robot.world.get_light_cube(LightCube1Id)
cube2 = robot.world.get_light_cube(LightCube2Id)
cube3 = robot.world.get_light_cube(LightCube3Id)
retries = 5
i = 0
for i in range(0,5):
# Try to see at least 1 cube
behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
seen_cube = robot.world.wait_for_observed_light_cube(timeout=10,include_existing=True)
except:
seen_cube = None
behavior.stop()
# Try to observe possible stacked cubes
robot.set_head_angle(degrees(10)).wait_for_completed()
time.sleep(.5)
robot.set_head_angle(degrees(30)).wait_for_completed()
time.sleep(.5)
robot.set_head_angle(degrees(0)).wait_for_completed()
num_observed_cubes = 0
if(cube1.pose.is_comparable(robot.pose)):
num_observed_cubes += 1
if(cube2.pose.is_comparable(robot.pose)):
num_observed_cubes += 1
if(cube3.pose.is_comparable(robot.pose)):
num_observed_cubes += 1
if(num_observed_cubes == 3):
# All cubes were observed, check if stacked
ref = []
ref.append(cube1.pose.position.x)
ref.append(cube1.pose.position.y)
tol = 20 # Less than 20 mm means that the cubes are stacked
delta2 = math.sqrt((ref[0]-cube2.pose.position.x)**2 + (ref[1]-cube2.pose.position.y)**2)
delta3 = math.sqrt((ref[0]-cube3.pose.position.x)**2 + (ref[1]-cube3.pose.position.y)**2)
if(delta2 < tol and delta3 < tol):
try:
behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.KnockOverCubes)
behavior.wait_for_started(timeout=10)
behavior.wait_for_completed(timeout=None)
except asyncio.TimeoutError:
print('WARNING: Timeout exception raised from behavior type KnockOverCubes.')
except:
print('WARNING: An exception was raised from behavior type KnockOverCubes.')
else:
robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed()
return 1
else:
robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed()
return 1
if(i >= 4):
return 0
def main(robot: cozmo.robot.Robot):
# first run
robot.say_text("I am starting").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabWhoa,
in_parallel=True).wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300),
should_play_anim=True,
in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
# second run
robot.say_text("second checkpoint").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabWhew).wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
robot.turn_in_place(degrees(495)).wait_for_completed()
# third run
robot.play_anim_trigger(
cozmo.anim.Triggers.DizzyShakeStop).wait_for_completed()
robot.say_text("third checkpoint").wait_for_completed()
robot.drive_straight(distance_mm(283),
speed_mmps(300)).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
# fourth run
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabWin).wait_for_completed()
robot.say_text("fourth checkpoint").wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
# fifth run
robot.say_text("fifth checkpoint").wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabZombie).wait_for_completed()
# sixth run
robot.say_text("sixth checkpoint").wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabAmazed).wait_for_completed()
# seventh run
robot.say_text("seventh checkpoint").wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
robot.turn_in_place(degrees(135)).wait_for_completed()
# eight run
robot.say_text("eighth checkpoint").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CozmoSaysGetOut).wait_for_completed()
robot.drive_straight(distance_mm(200),
speed_mmps(300)).wait_for_completed()
# the end
robot.say_text("I reached the end!").wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabHappy).wait_for_completed()
notes = [
cozmo.song.SongNote(cozmo.song.NoteTypes.C2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.C2_Sharp,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.D2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.D2_Sharp,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.E2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.F2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.F2_Sharp,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2_Sharp,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2_Sharp,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.C3,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.C3_Sharp,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.Rest,
cozmo.song.NoteDurations.Quarter)
]
# Play the ascending notes
robot.play_song(notes, loop_count=1).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
get_ready(robot)
#when see face, start
face = None
while True:
#if sees Mrs. H, gets root beer (not working yet for specific person)
if face and face.is_visible:
print("Ctrl-c to quit")
#play happy animation
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabHappy).wait_for_completed()
#say something
robot.say_text("Howdy Mrs. H. Let me get you a root beer."
).wait_for_completed()
#light up cube1...with spin?
cube1 = robot.world.get_light_cube(LightCube1Id)
for i in range(4):
cube1.set_light_corners(cozmo.lights.red_light,
cozmo.lights.off_light,
cozmo.lights.off_light,
cozmo.lights.off_light)
time.sleep(0.1)
cube1.set_light_corners(cozmo.lights.off_light,
cozmo.lights.red_light,
cozmo.lights.off_light,
cozmo.lights.off_light)
time.sleep(0.1)
cube1.set_light_corners(cozmo.lights.off_light,
cozmo.lights.off_light,
cozmo.lights.red_light,
cozmo.lights.off_light)
time.sleep(0.1)
cube1.set_light_corners(cozmo.lights.off_light,
cozmo.lights.off_light,
cozmo.lights.off_light,
cozmo.lights.red_light)
time.sleep(0.1)
#set cube lights to red
cube1.set_lights(cozmo.lights.red_light)
#get lift out of way. not stopping at 20%
robot.move_lift(10)
robot.set_lift_height(0.2, in_parallel=True).wait_for_completed()
#drive
robot.drive_straight(distance_mm(400),
speed_mmps(100)).wait_for_completed()
robot.move_lift(-1)
time.sleep(3)
#dock with cube...NOT WORKING
robot.move_lift(3)
#bring drink back
robot.drive_straight(distance_mm(-400),
speed_mmps(100)).wait_for_completed()
#put drink down
robot.move_lift(-1)
#animation
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabBored).wait_for_completed()
#say something
robot.say_text(
"Here you go. Enjoy your soda.").wait_for_completed()
#after program, move head down
robot.move_head(0.50)
time.sleep(3) #3 seconds
else:
robot.set_backpack_lights_off()
# Wait until we we can see another face
try:
face = robot.world.wait_for_observed_face(timeout=30)
except asyncio.TimeoutError:
print("Didn't find a face.")
return
time.sleep(.2)
async def __execute_directions(self, directions):
print("Robot is at: ", self.robot.pose)
await self.robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to X: ", self.robot.pose)
await self.robot.drive_straight(distance=distance_mm(directions.position.x * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("ROBOT is at AFTER DRIVING in the X direction: ", self.robot.pose)
await self.robot.turn_in_place(angle=degrees(90)).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to Y: ", self.robot.pose)
await self.robot.drive_straight(distance=distance_mm(directions.position.y * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("ROBOT is at AFTER DRIVING in the Y direction: ", self.robot.pose)
def cozmo_program(robot: cozmo.robot.Robot):
# Use a "for loop" to repeat the indented code 4 times
# Note: the _ variable name can be used when you don't need the value
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()
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, goal_pose
global grid, gui, pf
global camera_settings
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
await robot.set_head_angle(cozmo.util.degrees(3)).wait_for_completed()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
camera_settings = np.array([
[fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]
], dtype=np.float)
###################
# pickup point
#pickup_node = Node((153, 240))
# dropoff_node = Node((544, 344))
# localize the robot
await look_around_until_converge(robot)
# intialize an explorer after localized
#cosimo = CozmoExplorer(robot, x_0=last_pose.position.x, y_0=last_pose.position.y, theta_0=last_pose.rotation.angle_z.radians)
# move robot to pickup zone once localized
print("LAST POSE IS:", last_pose)
#print("COZMO CONVERTED THAT TO A START AT:", cosimo.last_arena_pose)
directions = goal_pose - last_pose
current_pose = last_pose
last_robot_pose = robot.pose
print("SETTING LAST ROBOT POSE TO: ", last_robot_pose)
print("SO WE GOING TO FOLLOW THIS TO PICKUP ZONE:", directions)
await execute_directions(robot, directions)
await robot.turn_in_place(angle=cozmo.util.Angle(degrees=45)).wait_for_completed()
print("LAST ROBOT POSE IS: ", last_robot_pose)
print("CURRENT POSE IS:", robot.pose)
print("WE THINK WE MOVED THIS MUCH TO GO TO PICKUP ZONE: ", convertPoseToInches(robot.pose - last_robot_pose))
current_pose = current_pose + convertPoseToInches(rotate_point(robot.pose, - last_robot_pose)
last_robot_pose = robot.pose
print("COZMO THINKS IT IS AT AFTER DRIVING TO PICKUPZONE: ", current_pose)
# await robot.say_text('Ready for pick up!').wait_for_completed()
drop_off_directions = [(3, 4.5, 0), (21.75, 4.5, 90), (21.75, 13.75, 90)]
pick_up_directions = [(21.75, 4.5, 90), (3, 4.5, 0), (4.5, 20)]
while True:
cube = await robot.world.wait_for_observed_light_cube(timeout=30)
print("Found cube: %s" % cube)
await robot.pickup_object(cube, num_retries=5).wait_for_completed()
current_pose = current_pose + convertPoseToInches(robot.pose - last_robot_pose)
print("WE THINK WE MOVED THIS MUCH TO PICK UP CUBE: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
#cosimo.update_pose()
print("COZMO THINKS IT IS AT AFTER PICKING UP CUBE: ", current_pose)
#await look_around_until_converge(robot)
# intialize an explorer after localized
#cosimo = CozmoExplorer(robot, x_0=last_pose.position.x, y_0=last_pose.position.y, theta_0=last_pose.rotation.angle_z.radians)
# move robot to pickup zone once localized
#print("COZMO CONVERTED THAT TO A START AT:", cosimo.last_arena_pose)
#current_pose = last_pose
# rrt to drop zone and drop off cube
for destination in drop_off_directions:
directions = convertInchesToPose(destination) - current_pose
await execute_directions(robot,directions)
current_pose = current_pose + convertPoseToInches(robot.pose - last_robot_pose)
print("WE THINK WE MOVED THIS MUCH TO FOLLOW DIRECTIONS: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
print("COZMO THINKS IT IS AT AFTER FOLLOWING DIRECTIONS: ", current_pose)
#await cosimo.go_to_goal(goal_node=dropoff_node)
await robot.set_lift_height(0.0).wait_for_completed()
# rrt to just in front of pick up zone
# await cosimo.go_to_goal(goal_node=pickup_node)
def CozmoWarehouseWorker:
def __init__(self, robot, current_arena_pose, ):
self.current_arena_pose = current_arena_pose
self.last_robot_pose = robot.pose
self.robot = robot
async def execute_directions(directions):
print("Robot is at: ", self.robot.pose)
await self.robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to X: ", self.robot.pose)
await self.robot.drive_straight(distance=distance_mm(directions.position.x * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("ROBOT is at AFTER DRIVING in the X direction: ", self.robot.pose)
await self.robot.turn_in_place(angle=cozmo.util.Angle(degrees=90)).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to Y: ", self.robot.pose)
await self.robot.drive_straight(distance=distance_mm(directions.position.y * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("ROBOT is at AFTER DRIVING in the Y direction: ", self.robot.pose)
async def look_around_until_converge(robot: cozmo.robot.Robot):
# globals
global flag_odom_init, last_pose
global grid, gui, pf
# reset variables
conf = False
last_pose = cozmo.util.Pose(0,0,0,angle_z=cozmo.util.Angle(degrees=0))
pf = ParticleFilter(grid)
# reset lift and head
await robot.set_lift_height(0.0).wait_for_completed()
await robot.set_head_angle(cozmo.util.degrees(3)).wait_for_completed()
while not conf:
if (await is_picked_up(robot)):
continue
# move a little
last_pose = robot.pose
await robot.turn_in_place(angle=cozmo.util.Angle(degrees=20)).wait_for_completed()
curr_pose = robot.pose
detected_markers, camera_image = await marker_processing(robot, camera_settings)
# update, motion, and measurment with the odometry and marker data
odometry = compute_odometry(curr_pose)
curr_x, curr_y, curr_h, conf = pf.update(odometry, detected_markers)
# update gui
gui.show_particles(pf.particles)
gui.show_mean(curr_x, curr_y, curr_h)
gui.show_camera_image(camera_image)
gui.updated.set()
last_pose = cozmo.util.Pose(curr_x , curr_y, 0, angle_z=cozmo.util.Angle(degrees=curr_h))
return last_pose
class CozmoThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self, daemon=False)
def run(self):
cozmo.robot.Robot.drive_off_charger_on_connect = False # Cozmo can stay on his charger
cozmo.run_program(run, use_viewer=False)
if __name__ == '__main__':
# cozmo thread
cozmo_thread = CozmoThread()
cozmo_thread.start()
# init
gui.show_particles(pf.particles)
gui.show_mean(0, 0, 0)
gui.start()
async def desk_security_guard(robot):
'''The core of the desk_security_guard program'''
# Turn on image receiving by the camera
robot.camera.image_stream_enabled = True
# Connect Twitter, run async in the background
twitter_api, twitter_auth = twitter_helpers.init_twitter(twitter_keys)
stream_to_app_comms = TwitterStreamToAppCommunication()
stream_listener = SecurityGuardStreamListener(twitter_api, stream_to_app_comms)
twitter_stream = twitter_helpers.CozmoStream(twitter_auth, stream_listener)
twitter_stream.async_userstream(_with='user')
# Create our security guard
dsg = DeskSecurityGuard(twitter_api)
# Make sure Cozmo is clear of the charger
if robot.is_on_charger:
# Drive fully clear of charger (not just off the contacts)
await robot.drive_off_charger_contacts().wait_for_completed()
await robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
# Tilt head up to look for people
await robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
initial_pose_angle = robot.pose_angle
patrol_offset = 0 # middle
max_pose_angle = 45 # offset from initial pose_angle (up to +45 or -45 from this)
# Time to wait between each turn and patrol, in seconds
time_between_turns = 2.5
time_between_patrols = 20
time_for_next_turn = time.time() + time_between_turns
time_for_next_patrol = time.time() + time_between_patrols
while True:
# Handle any external requests to arm or disarm Cozmo
if stream_to_app_comms.has_arm_request:
stream_to_app_comms.has_arm_request = False
if not dsg.is_armed:
print("Alarm Armed")
dsg.is_armed = True
if stream_to_app_comms.has_disarm_request:
stream_to_app_comms.has_disarm_request = False
if dsg.is_armed:
print("Alarm Disarmed")
dsg.is_armed = False
stream_to_app_comms.is_armed = dsg.is_armed
# Turn head every few seconds to cover a wider field of view
# Only do this if not currently investigating an intruder
if (time.time() > time_for_next_turn) and not dsg.is_investigating_intruder():
# pick a random amount to turn
angle_to_turn = randint(10,40)
# 50% chance of turning in either direction
if randint(0,1) > 0:
angle_to_turn = -angle_to_turn
# Clamp the amount to turn
face_angle = (robot.pose_angle - initial_pose_angle).degrees
face_angle += angle_to_turn
if face_angle > max_pose_angle:
angle_to_turn -= (face_angle - max_pose_angle)
elif face_angle < -max_pose_angle:
angle_to_turn -= (face_angle + max_pose_angle)
# Turn left/right
await robot.turn_in_place(degrees(angle_to_turn)).wait_for_completed()
# Tilt head up/down slightly
await robot.set_head_angle(degrees(randint(30,44))).wait_for_completed()
# Queue up the next time to look around
time_for_next_turn = time.time() + time_between_turns
# Every now and again patrol left and right between 3 patrol points
if (time.time() > time_for_next_patrol) and not dsg.is_investigating_intruder():
# Check which way robot is facing vs initial pose, pick a new patrol point
face_angle = (robot.pose_angle - initial_pose_angle).degrees
drive_right = (patrol_offset < 0) or ((patrol_offset == 0) and (face_angle > 0))
# Turn to face the new patrol point
if drive_right:
await robot.turn_in_place(degrees(90 - face_angle)).wait_for_completed()
patrol_offset += 1
else:
await robot.turn_in_place(degrees(-90 - face_angle)).wait_for_completed()
patrol_offset -= 1
# Drive to the patrol point, playing animations along the way
await robot.drive_wheels(20, 20)
for i in range(1,4):
await robot.play_anim("anim_hiking_driving_loop_0" + str(i)).wait_for_completed()
# Stop driving
robot.stop_all_motors()
# Turn to face forwards again
face_angle = (robot.pose_angle - initial_pose_angle).degrees
if face_angle > 0:
await robot.turn_in_place(degrees(-90)).wait_for_completed()
else:
await robot.turn_in_place(degrees(90)).wait_for_completed()
# Queue up the next time to patrol
time_for_next_patrol = time.time() + time_between_patrols
# look for intruders
await check_for_intruder(robot, dsg)
# Sleep to allow other things to run
await asyncio.sleep(0.05)
def clean_up_cubes():
global cubeIDs,SIDE
cubeIDs = []
switch_cube_off(robot.world.get_light_cube(LightCube1Id))
switch_cube_off(robot.world.get_light_cube(LightCube2Id))
switch_cube_off(robot.world.get_light_cube(LightCube3Id))
go_to_charger()
turn_around()
# I. Find first cube and put it next to charger
cube = look_for_next_cube()
if not cube:
return
switch_cube_on(cube)
print(cubeIDs)
success = pickUp_cube(cube)
if not success:
switch_cube_off(cube)
return
charger = go_to_charger()
final_adjust(charger,60)
robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed()
putDown_cube(cube)
switch_cube_off(cube)
# Get back in front of charger
robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed()
robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed()
robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed()
# II. Find next cube and stack on first placed cube
cube = look_for_next_cube()
if(cube == None):
return
switch_cube_on(cube)
print(cubeIDs)
success = pickUp_cube(cube)
if not success:
switch_cube_off(cube)
return
charger = go_to_charger()
robot.turn_in_place(degrees(SIDE*70)).wait_for_completed() #SIDE*180
stack_cube(cubes[0])
switch_cube_off(cube)
# Get back in front of charger
#robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed()
charger = go_to_charger()
final_adjust(charger,100,80)
robot.turn_in_place(degrees(SIDE*180)).wait_for_completed() #-SIDE*70
robot.set_lift_height(height=0,max_speed=10,in_parallel=False).wait_for_completed()
#robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed()
# III. Go get the last cube and lay it in front of the others
cube = look_for_next_cube()
if(cube == None):
return
switch_cube_on(cube)
print(cubeIDs)
success = pickUp_cube(cube)
if not success:
switch_cube_off(cube)
return
turn_around()
charger = go_to_charger()
final_adjust(charger,80)
robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed()
putDown_cube(cube)
switch_cube_off(cube)
# Get back in front of charger
robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed()
robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed()
robot.drive_straight(distance_mm(50),speed_mmps(40)).wait_for_completed()
turn_around()
return
def mainLoop(robot: cozmo.robot.Robot):
threadFace = threading.Thread(target=face_follower.follow_faces,
args=(robot, ))
threadFace.start()
while True:
print('Expression:' + face_follower.notifyExpression())
# In a loop, we grab the user input
print("Listening...")
humanString = (voiceParse.parseVoice()).lower()
ListOfCommand = [str(s) for s in (humanString.lower()).split()]
# Check it for a quit condition.
if {'shut', 'down', 'cozmo'} <= set(ListOfCommand) or {
'cosmo', 'shut', 'down'
} <= set(ListOfCommand):
# If we quit, we log the quit and leave the program.
robot.say_text("Ok, shut down",
in_parallel=True).wait_for_completed()
docking_cozmo(robot)
ListOfCommand.clear()
addEntry(log, "Conversation ended.")
ListOfCommand.clear()
sys.exit()
if robot.battery_voltage <= 2.0:
print('Running out of battery')
robot.say_text("Running out of battery",
in_parallel=True).wait_for_completed()
docking_cozmo(robot)
if {'go', 'out', 'charger'} <= set(ListOfCommand):
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(60)).wait_for_completed()
ListOfCommand.clear()
if 'open reddit' in humanString:
reg_ex = re.search('open reddit(.*)', humanString.lower())
url = 'https://www.reddit.com/'
addEntry(log, "Human says: " + humanString)
print("Human says: " + humanString)
if reg_ex:
subreddit = reg_ex.group(1)
url = url + 'r/' + subreddit
webbrowser.open(url)
addEntry(log, "Cozmo says: " + humanString)
print("Cozmo says: " + "Done")
robot.say_text("Done", in_parallel=True).wait_for_completed()
continue
if {'go', 'to', 'charger'} <= set(ListOfCommand):
docking_cozmo(robot)
ListOfCommand.clear()
if 'open google' in humanString:
reg_ex = re.search('open google(.*)', humanString.lower())
url = 'https://www.google.com/'
if reg_ex:
subreddit = reg_ex.group(1)
url = url + 'search?q=' + subreddit
webbrowser.open(url)
addEntry(log, "Human says: " + humanString)
print("Human says: " + humanString)
addEntry(log, "Cozmo says: " + humanString)
print("Cozmo says: " + "Done")
robot.say_text("Done", in_parallel=True).wait_for_completed()
continue
if 'open my favorite song' in humanString:
robot.say_text("What song?", in_parallel=True).wait_for_completed()
humanString1 = (voiceParse.parseVoice()).lower()
print("Human says: " + humanString1)
reg_ex = re.search('(.+)', humanString1)
if reg_ex:
SongName = reg_ex.group(1)
#you can add any song you want in the ListofSongs, by adding key word with its watch?v=?????????
json_file = open('texttest.json')
json_str = json_file.read()
dictOfSong = ast.literal_eval(json_str)
"""ListofSongs = {"part of your world": "watch?v=gtpLsPPtC88", "full nocturne": 'watch?v=liTSRH4fix4',
'eyes on fire': 'watch?v=LAxCqlU-OAo', 'nocturne in f minor': 'watch?v=E3qHO9aOQYM',
'moonlight sonata': 'watch?v=4Tr0otuiQuU', 'clair de lune': 'watch?v=ea2WoUtbzuw',
'hello': 'watch?v=YQHsXMglC9A', 'skyfall': 'watch?v=DeumyOzKqgI'}"""
subUrl = 'https://www.youtube.com/search?q=' + SongName
has_Song = False
for x in range(len(dictOfSong['song'])):
if (dictOfSong['song'][x]['name']).lower() == SongName:
webbrowser.open(dictOfSong['song'][x]['url'])
has_Song = True
break
if has_Song == False:
webbrowser.open(subUrl)
continue
continue
elif type(SongName) == type(None):
robot.say_text("No command found",
in_parallel=True).wait_for_completed()
continue
robot.say_text("Done", in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + "Done")
print("Cozmo says: " + "Done")
continue
if 'open website' in humanString:
robot.say_text("What do you want me to open",
in_parallel=True).wait_for_completed()
humanString1 = (voiceParse.parseVoice()).lower()
print("Human says: " + humanString1)
reg_ex = re.search('(.+)', humanString1)
if reg_ex:
domain = reg_ex.group(1)
url = 'https://www.' + domain + ".com"
ListofDictionaries = {
"google": "search?q=",
"youtube": 'search?q=',
"reddit": "r/",
'amazon': 's?url=search-alias%3Daps&field-keywords='
}
if domain not in str(ListofDictionaries):
webbrowser.open(url)
continue
robot.say_text("What are you looking for in " + domain,
in_parallel=True).wait_for_completed()
addEntry(
log,
"Cozmo says: " + "What are you looking for in " + domain)
print("Cozmo says: " + "What are you looking for in " + domain)
humanString2 = (voiceParse.parseVoice()).lower()
addEntry(log, "Human says: " + humanString2)
print("Human says: " + humanString2)
if "nothing" in humanString2:
webbrowser.open(url)
else:
newurl = url + "/" + ListofDictionaries.get(
domain) + humanString2
webbrowser.open(newurl)
else:
pass
robot.say_text("Done", in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + "Done")
print("Cozmo says: " + "Done")
continue
if ({"cosmo", "email"} <= set(ListOfCommand)) or ({"cozmo", "email"} <=
set(ListOfCommand)):
ListOfCommand.clear()
robot.say_text("Who is the recipient?",
in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + "Who is the recipient?")
print("Cozmo says: " + "Who is the recipient?")
recipient = (voiceParse.parseVoice()).lower()
print(recipient)
addEntry(log, "Human says: " + recipient)
print("Human says: " + recipient)
#you can add any email in the line below
"""ListofEmails = {"bright":"*****@*****.**","boss":"*****@*****.**",
"tim":"*****@*****.**","lucky":"*****@*****.**"}"""
json_file1 = open('emailtext.json')
json_str1 = json_file1.read()
dictOfEmail = ast.literal_eval(json_str1)
has_Email = False
for x in range(len(dictOfEmail['email'])):
print((dictOfEmail['email'][x]['name']).lower())
if (dictOfEmail['email'][x]['name']).lower() == recipient:
robot.say_text("What should I say?",
in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + "What should I say?")
print("Cozmo says: " + "What should I say?")
content = (voiceParse.parseVoice()).lower()
addEntry(log, "Human says: " + content)
print("Human says: " + content)
# init gmail SMTP
ListOfSmtp = {
'gmail': [{
'name': 'smtp.gmail.com',
'key': '587'
}],
'outlook': [{
'name': 'smtp-mail.outlook.com',
'key': '587'
}],
'hotmail': [{
'name': 'smtp.live.com',
'key': '25'
}]
}
checkSmtp = dictOfEmail['email'][0]['url'].split(".")
b = str(checkSmtp[0]).split("@")
c = b[1].split(".")
d = c[0]
#check email whether our email is outlook or gmail
if d in ListOfSmtp:
mail = smtplib.SMTP(ListOfSmtp[d][0]['name'],
ListOfSmtp[d][0]['key'])
# smtp-mail.outlook.com
# identify to server
mail.ehlo()
# encrypt session
mail.starttls()
# In this line, we open text file from gui
json_file = open('useradding.json')
json_str = json_file.read()
dictOfUser = ast.literal_eval(json_str)
# In this line, put your email and password
mail.login(dictOfUser['email'][0]['name'],
dictOfUser['email'][0]['password'])
# send to the recipient
mail.sendmail('recipient',
dictOfEmail['email'][x]['url'],
content + "\n" + "sent via Cozmo")
# end mail connection
mail.close()
robot.say_text("Sent",
in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + "Sent")
print("Cozmo says: " + "Sent")
has_Email = True
break
else:
robot.say_text(
"I can\'t send because I don't know the SMTP name",
in_parallel=True).wait_for_completed()
addEntry(
log, "Cozmo says: " +
"I can\'t send because I don't know the SMTP name")
print(
"Cozmo says: " +
"I can\'t send because I don't know the SMTP name")
return mainLoop()
if has_Email == False:
robot.say_text("I don\'t know him",
in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + "I don\'t know him")
print("Cozmo says: " + "I don\'t know him")
continue
if 'weather forecast in' in humanString:
addEntry(log, "Human says: " + humanString)
print("Human says: " + humanString)
reg_ex = re.search('weather forecast in (.*)', humanString)
if reg_ex:
city = reg_ex.group(1)
weather = Weather()
location = weather.lookup_by_location(city)
forecasts = location.forecast
for i in range(0, 3):
TempetureForecast = (
'On %s will it %s. The maximum temperture will be %.1f degrees Celcius.'
'The lowest temperature will be %.1f degrees Celcius.'
% (forecasts[i].date, forecasts[i].text,
int(forecasts[i].high), int(forecasts[i].low)))
robot.say_text(TempetureForecast,
use_cozmo_voice=True,
duration_scalar=1,
in_parallel=True).wait_for_completed()
addEntry(log, "Cozmo says: " + TempetureForecast)
print("Cozmo says: " + TempetureForecast)
continue
if ({"cosmo", "joke"} <= set(ListOfCommand)) or ({"cozmo", "joke"} <=
set(ListOfCommand)):
ListOfCommand.clear()
res = requests.get('https://icanhazdadjoke.com/',
headers={"Accept": "application/json"})
if res.status_code == requests.codes.ok:
robot.say_text(str(res.json()['joke']),
in_parallel=True,
duration_scalar=1.2).wait_for_completed()
functionsCozmo.motion_step(robot)
print("Cozmo says: " + str(res.json()['joke']))
addEntry(log, "Cozmo says: " + str(res.json()['joke']))
else:
robot.say_text(str(res.json()['oops!I ran out of jokes']),
in_parallel=True).wait_for_completed()
print("Cozmo says: " + 'oops!I ran out of jokes')
addEntry(log, "Cozmo says: " + 'oops!I ran out of jokes')
continue
#Action Commands
if ({"sing", "song", "cosmo", "stupid"} <= set(ListOfCommand)) or (
{"sing", "song", "cozmo", "stupid"} <= set(ListOfCommand)):
addEntry(log, "Human says: " + humanString)
print("Human says: " + humanString)
ListOfCommand.clear()
functionsCozmo.cozmo_singing(robot)
robot.say_text("Do you like my song?",
play_excited_animation=True,
in_parallel=True).wait_for_completed()
print("Cozmo says: " + "Do you like my song?")
addEntry(log, "Cozmo says: " + "Do you like my song?")
continue
if ({"random", "cosmo", 'emotion'} <= set(ListOfCommand)) or (
{"random", "cozmo", 'emotion'} <= set(ListOfCommand)):
ListOfCommand.clear()
functionsCozmo.motion_step(robot)
continue
# Else, we log what the human said.
addEntry(log, "Human says: " + humanString)
print("Human says: " + humanString)
# Grab the response from CleverBot
cozmoString = cleverbot.say(humanString)
# Print the response to the screen and add it to the log
print("Cozmo says: " + cozmoString)
addEntry(log, "Cozmo says: " + cozmoString)
# Then we make Cozmo say it.
robot.say_text(cozmoString, in_parallel=True).wait_for_completed()
ListOfCommand.clear()
async def desk_security_guard(robot):
'''The core of the desk_security_guard program'''
# Turn on image receiving by the camera
robot.camera.image_stream_enabled = True
# Connect Twitter, run async in the background
twitter_api, twitter_auth = twitter_helpers.init_twitter(twitter_keys)
stream_to_app_comms = TwitterStreamToAppCommunication()
stream_listener = SecurityGuardStreamListener(twitter_api, stream_to_app_comms)
twitter_stream = twitter_helpers.CozmoStream(twitter_auth, stream_listener)
twitter_stream.async_userstream(_with='user')
# Create our security guard
dsg = DeskSecurityGuard(twitter_api)
# Make sure Cozmo is clear of the charger
if robot.is_on_charger:
# Drive fully clear of charger (not just off the contacts)
await robot.drive_off_charger_contacts().wait_for_completed()
await robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
# Tilt head up to look for people
await robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
initial_pose_angle = robot.pose_angle
patrol_offset = 0 # middle
max_pose_angle = 45 # offset from initial pose_angle (up to +45 or -45 from this)
# Time to wait between each turn and patrol, in seconds
time_between_turns = 2.5
time_between_patrols = 20
time_for_next_turn = time.time() + time_between_turns
time_for_next_patrol = time.time() + time_between_patrols
while True:
# Handle any external requests to arm or disarm Cozmo
if stream_to_app_comms.has_arm_request:
stream_to_app_comms.has_arm_request = False
if not dsg.is_armed:
print("Alarm Armed")
dsg.is_armed = True
if stream_to_app_comms.has_disarm_request:
stream_to_app_comms.has_disarm_request = False
if dsg.is_armed:
print("Alarm Disarmed")
dsg.is_armed = False
stream_to_app_comms.is_armed = dsg.is_armed
# Turn head every few seconds to cover a wider field of view
# Only do this if not currently investigating an intruder
if (time.time() > time_for_next_turn) and not dsg.is_investigating_intruder():
# pick a random amount to turn
angle_to_turn = randint(10,40)
# 50% chance of turning in either direction
if randint(0,1) > 0:
angle_to_turn = -angle_to_turn
# Clamp the amount to turn
face_angle = (robot.pose_angle - initial_pose_angle).degrees
face_angle += angle_to_turn
if face_angle > max_pose_angle:
angle_to_turn -= (face_angle - max_pose_angle)
elif face_angle < -max_pose_angle:
angle_to_turn -= (face_angle + max_pose_angle)
# Turn left/right
await robot.turn_in_place(degrees(angle_to_turn)).wait_for_completed()
# Tilt head up/down slightly
await robot.set_head_angle(degrees(randint(30,44))).wait_for_completed()
# Queue up the next time to look around
time_for_next_turn = time.time() + time_between_turns
# Every now and again patrol left and right between 3 patrol points
if (time.time() > time_for_next_patrol) and not dsg.is_investigating_intruder():
# Check which way robot is facing vs initial pose, pick a new patrol point
face_angle = (robot.pose_angle - initial_pose_angle).degrees
drive_right = (patrol_offset < 0) or ((patrol_offset == 0) and (face_angle > 0))
# Turn to face the new patrol point
if drive_right:
await robot.turn_in_place(degrees(90 - face_angle)).wait_for_completed()
patrol_offset += 1
else:
await robot.turn_in_place(degrees(-90 - face_angle)).wait_for_completed()
patrol_offset -= 1
# Drive to the patrol point, playing animations along the way
await robot.drive_wheels(20, 20)
for i in range(1,4):
await robot.play_anim("anim_hiking_driving_loop_0" + str(i)).wait_for_completed()
# Stop driving
robot.stop_all_motors()
# Turn to face forwards again
face_angle = (robot.pose_angle - initial_pose_angle).degrees
if face_angle > 0:
await robot.turn_in_place(degrees(-90)).wait_for_completed()
else:
await robot.turn_in_place(degrees(90)).wait_for_completed()
# Queue up the next time to patrol
time_for_next_patrol = time.time() + time_between_patrols
# look for intruders
await check_for_intruder(robot, dsg)
# Sleep to allow other things to run
await asyncio.sleep(0.05)
def straight(robot, distance):
global isTakingPicture
isTakingPicture = False
hello(robot)
robot.drive_straight(distance_mm(distance), speed_mmps(50)).wait_for_completed()
isTakingPicture = False
def update_current_arena_pose(self):
local_to_arena_angle_diff = diff_heading_deg(self.last_robot_pose.rotation.degrees, self.current_arena_pose.rotation.degrees)
arena_initial_pose_mm = rotate_point(self.last_robot_pose.position.x, self.last_robot_pose.position.y, local_to_arena_angle_diff)
arena_final_pose_mm = rotate_point(self.robot.pose.position.x, self.robot.pose.position.y )
self.current_arena_pose = self.current_arena_pose + convertPoseFromMmToInches(rotate_point(self.robot.pose, - last_robot_pose)
async def __execute_directions(self, directions):
print("Robot is at: ", self.robot.pose)
await self.robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to X: ", self.robot.pose)
await self.robot.drive_straight(distance=distance_mm(directions.position.x * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("ROBOT is at AFTER DRIVING in the X direction: ", self.robot.pose)
await self.robot.turn_in_place(angle=degrees(90)).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to Y: ", self.robot.pose)
await self.robot.drive_straight(distance=distance_mm(directions.position.y * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("ROBOT is at AFTER DRIVING in the Y direction: ", self.robot.pose)
async def localize(self):
# reset our location estimates
conf = False
self.current_arena_pose = Pose(0,0,0,angle_z=degrees(0))
self.pf = ParticleFilter(grid)
# reset lift and head
await self.robot.set_lift_height(0.0).wait_for_completed()
await self.robot.set_head_angle(degrees(3)).wait_for_completed()
while not conf:
# move a little
self.last_robot_pose = self.robot.pose
await self.robot.turn_in_place(angle=degrees(20)).wait_for_completed()
odometry = self.__compute_odometry()
detected_markers, camera_image = await self.__marker_processing()
# update, motion, and measurment with the odometry and marker data
curr_x, curr_y, curr_h, conf = pf.update(odometry, detected_markers)
# update gui
self.gui.show_particles(self.pf.particles)
self.gui.show_mean(curr_x, curr_y, curr_h)
self.gui.show_camera_image(camera_image)
self.gui.updated.set()
self.current_arena_pose = Pose(curr_x , curr_y, 0, angle_z=degrees(curr_h))
def __compute_odometry(self, cvt_inch=True):
'''
Compute the odometry given the current pose of the robot (use robot.pose)
Input:
- curr_pose: a cozmo.robot.Pose representing the robot's current location
- cvt_inch: converts the odometry into grid units
Returns:
- 3-tuple (dx, dy, dh) representing the odometry
'''
last_x, last_y, last_h = self.last_robot_pose.position.x, self.last_robot_pose.position.y, \
self.last_robot_pose.rotation.angle_z.degrees
curr_x, curr_y, curr_h = self.robot.pose.position.x, self.robot.pose.position.y, \
self.robot.pose.rotation.angle_z.degrees
dx, dy = rotate_point(curr_x-last_x, curr_y-last_y, -last_h)
if cvt_inch:
dx, dy = dx / grid.scale, dy / grid.scale
return (dx, dy, diff_heading_deg(curr_h, last_h))
async def __marker_processing(self, show_diagnostic_image=False):
'''
Obtain the visible markers from the current frame from Cozmo's camera.
Since this is an async function, it must be called using await, for example:
markers, camera_image = await marker_processing(robot, camera_settings, show_diagnostic_image=False)
Input:
- robot: cozmo.robot.Robot object
- camera_settings: 3x3 matrix representing the camera calibration settings
- show_diagnostic_image: if True, shows what the marker detector sees after processing
Returns:
- a list of detected markers, each being a 3-tuple (rx, ry, rh)
(as expected by the particle filter's measurement update)
- a PIL Image of what Cozmo's camera sees with marker annotations
'''
# Wait for the latest image from Cozmo
image_event = await self.robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
# Convert the image to grayscale
image = np.array(image_event.image)
image = color.rgb2gray(image)
# Detect the markers
markers, diag = detect.detect_markers(image, self.camera_settings, include_diagnostics=True)
# Measured marker list for the particle filter, scaled by the grid scale
marker_list = [marker['xyh'] for marker in markers]
marker_list = [(x/self.grid.scale, y/self.grid.scale, h) for x,y,h in marker_list]
# Annotate the camera image with the markers
if not show_diagnostic_image:
annotated_image = image_event.image.resize((image.shape[1] * 2, image.shape[0] * 2))
annotator.annotate_markers(annotated_image, markers, scale=2)
else:
diag_image = color.gray2rgb(diag['filtered_image'])
diag_image = Image.fromarray(np.uint8(diag_image * 255)).resize((image.shape[1] * 2, image.shape[0] * 2))
annotator.annotate_markers(diag_image, markers, scale=2)
annotated_image = diag_image
return marker_list, annotated_image
async def run(robot: cozmo.robot.Robot):
cosimo = CozmoWarehouseWorker()
cosimo.localize()
cosimo.drive_to(cosimo.pick_up_pose)
directions = goal_pose - last_pose
current_pose = last_pose
last_robot_pose = robot.pose
print("SETTING LAST ROBOT POSE TO: ", last_robot_pose)
print("SO WE GOING TO FOLLOW THIS TO PICKUP ZONE:", directions)
await execute_directions(robot, directions)
await robot.turn_in_place(angle=degrees(45)).wait_for_completed()
print("LAST ROBOT POSE IS: ", last_robot_pose)
print("CURRENT POSE IS:", robot.pose)
print("WE THINK WE MOVED THIS MUCH TO GO TO PICKUP ZONE: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
print("COZMO THINKS IT IS AT AFTER DRIVING TO PICKUPZONE: ", current_pose)
# await robot.say_text('Ready for pick up!').wait_for_completed()
while True:
cube = await robot.world.wait_for_observed_light_cube(timeout=30)
print("Found cube: %s" % cube)
await robot.pickup_object(cube, num_retries=5).wait_for_completed()
current_pose = current_pose + convertPoseToInches(robot.pose - last_robot_pose)
print("WE THINK WE MOVED THIS MUCH TO PICK UP CUBE: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
#cosimo.update_pose()
print("COZMO THINKS IT IS AT AFTER PICKING UP CUBE: ", current_pose)
#await look_around_until_converge(robot)
# intialize an explorer after localized
#cosimo = CozmoExplorer(robot, x_0=last_pose.position.x, y_0=last_pose.position.y, theta_0=last_pose.rotation.angle_z.radians)
# move robot to pickup zone once localized
#print("COZMO CONVERTED THAT TO A START AT:", cosimo.last_arena_pose)
#current_pose = last_pose
# rrt to drop zone and drop off cube
for destination in drop_off_directions:
directions = convertInchesToPose(destination) - current_pose
await execute_directions(robot,directions)
current_pose = current_pose + convertPoseToInches(robot.pose - last_robot_pose)
print("WE THINK WE MOVED THIS MUCH TO FOLLOW DIRECTIONS: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
print("COZMO THINKS IT IS AT AFTER FOLLOWING DIRECTIONS: ", current_pose)
#await cosimo.go_to_goal(goal_node=dropoff_node)
await robot.set_lift_height(0.0).wait_for_completed()
# rrt to just in front of pick up zone
# await cosimo.go_to_goal(goal_node=pickup_node)
class CozmoThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self, daemon=False)
def run(self):
cozmo.robot.Robot.drive_off_charger_on_connect = False # Cozmo can stay on his charger
cozmo.run_program(run, use_viewer=False)
if __name__ == '__main__':
# cozmo thread
cozmo_thread = CozmoThread()
cozmo_thread.start()
# init
gui.show_particles(pf.particles)
gui.show_mean(0, 0, 0)
gui.start()
def drive_to_charger(robot, trial):
'''The core of the drive_to_charger program'''
# If the robot was on the charger, drive them forward and clear of the charger
if robot.is_on_charger:
# drive off the charger
print("I am on the charger. Driving off the charger...")
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(70),
speed_mmps(50)).wait_for_completed()
# Start moving the lift down
robot.move_lift(-3)
# turn around to look at the charger
robot.turn_in_place(degrees(180)).wait_for_completed()
# Tilt the head to be level
robot.set_head_angle(degrees(0)).wait_for_completed()
# wait half a second to ensure Cozmo has seen the charger
time.sleep(0.5)
# drive backwards away from the charger
robot.drive_straight(distance_mm(-60),
speed_mmps(50)).wait_for_completed()
# try to find the charger
charger = None
# show the battery voltage
print("battery voltage currently is: %s" % robot.battery_voltage)
# see if Cozmo already knows where the charger is
if robot.world.charger:
if robot.world.charger.pose.origin_id == robot.pose.origin_id:
print("Cozmo already knows where the charger is!")
charger = robot.world.charger
else:
# Cozmo knows about the charger, but the pose is not based on the
# same origin as the robot (e.g. the robot was moved since seeing
# the charger) so try to look for the charger first
pass
if not charger:
# Tell Cozmo to look around for the charger
print("looking for the charger now...")
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=60)
print("Found charger: %s" % charger)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if charger:
# lift his arms to manouver
robot.set_lift_height(0.8, 0.8, 0.8, 0.1).wait_for_completed()
# Attempt to drive near to the charger, and then stop.
print("Trial number %s" % trial)
print("Going for the charger!!!")
# robot.go_to_pose(charger.pose, relative_to_robot=True).wait_for_completed()
# action = robot.go_to_object(charger, distance_mm(60.0))
action = robot.go_to_pose(charger.pose)
action.wait_for_completed()
print("Completed action: result = %s" % action)
robot.drive_straight(distance_mm(-30),
speed_mmps(50)).wait_for_completed()
print("Done.")
# Turn 180 (and 10) degrees, then goes backwards at full speed
print("Now the grand finalle: turn around and park!")
print("Turning...")
# robot.turn_in_place(degrees(190)).wait_for_completed()
robot.turn_in_place(degrees(95)).wait_for_completed()
robot.turn_in_place(degrees(95)).wait_for_completed()
# robot.turn_in_place(degrees(10)).wait_for_completed()
time.sleep(1)
print("Get out of the way: here I go!")
robot.drive_straight(distance_mm(-130),
speed_mmps(150)).wait_for_completed()
print("checking if I did it...")
if robot.is_on_charger:
print("I did it! Yay!")
else:
print("I did not manage to dock in the charger =(")
print("Trying again...")
robot.world.charger = None
print("let me go a little bit further to be easier to see...")
robot.drive_straight(distance_mm(90),
speed_mmps(50)).wait_for_completed()
trial += 1
if trial < 4:
drive_to_charger(robot, trial)
else:
print("tired of trying. Giving up =(")
async def execute_directions(robot, directions):
print("Robot is at: ", robot.pose)
await robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
print("Robot is at after turning to be parrael to X: ", robot.pose)
await robot.drive_straight(distance=distance_mm(directions.position.x * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("Robot is at after driving in the X dirrection: ", robot.pose)
await robot.turn_in_place(angle=cozmo.util.Angle(degrees=90)).wait_for_completed()
print("Robot is at after turning to the Y dirrection: ", robot.pose)
await robot.drive_straight(distance=distance_mm(directions.position.y * grid.scale), speed=speed_mmps(80)).wait_for_completed()
print("Robot is at after driving in the Y dirreciton: ", robot.pose)
def cozmo_program(robot: cozmo.robot.Robot):
notes = [
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.D2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.D2, cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2, cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2, cozmo.song.NoteDurations.Half) ]
robot.say_text('Beginning leg one').wait_for_completed()
robot.drive_straight(distance_mm(200), speed_mmps(250)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.say_text('Beginning leg two').wait_for_completed()
robot.drive_straight(distance_mm(200), speed_mmps(250)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.turn_in_place(degrees(135)).wait_for_completed()
robot.say_text('Beginning leg three').wait_for_completed()
robot.drive_straight(distance_mm(282.843), speed_mmps(250)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.say_text('Beginning leg four').wait_for_completed()
robot.drive_straight(distance_mm(200), speed_mmps(250)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.say_text('Beginning leg five and six').wait_for_completed()
robot.drive_straight(distance_mm(400), speed_mmps(250)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.say_text('Beginning leg seven').wait_for_completed()
robot.drive_straight(distance_mm(282.843), speed_mmps(250)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.turn_in_place(degrees(135)).wait_for_completed()
robot.say_text('Beginning leg eight').wait_for_completed()
robot.drive_straight(distance_mm(200), speed_mmps(250)).wait_for_completed()
robot.say_text('I have reached the end of the line').wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.FrustratedByFailureMajor).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.FacePlantRoll).wait_for_completed()
# robot.play_anim_trigger(cozmo.anim.Triggers.DizzyShakeLoop).wait_for_completed()
robot.play_song(notes, loop_count=1).wait_for_completed()
def drive_to_charger(robot):
'''The core of the drive_to_charger program'''
# If the robot was on the charger, drive them forward and clear of the charger
if robot.is_on_charger:
# drive off the charger
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(100),
speed_mmps(50)).wait_for_completed()
# Start moving the lift down
robot.move_lift(-3)
# turn around to look at the charger
robot.turn_in_place(degrees(180)).wait_for_completed()
# Tilt the head to be level
robot.set_head_angle(degrees(0)).wait_for_completed()
# wait half a second to ensure Cozmo has seen the charger
time.sleep(0.5)
# drive backwards away from the charger
robot.drive_straight(distance_mm(-60),
speed_mmps(50)).wait_for_completed()
# try to find the charger
charger = None
# see if Cozmo already knows where the charger is
if robot.world.charger:
if robot.world.charger.pose.is_comparable(robot.pose):
print("Cozmo already knows where the charger is!")
charger = robot.world.charger
else:
# Cozmo knows about the charger, but the pose is not based on the
# same origin as the robot (e.g. the robot was moved since seeing
# the charger) so try to look for the charger first
pass
if not charger:
# Tell Cozmo to look around for the charger
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=30)
print("Found charger: %s" % charger)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if charger:
# Attempt to drive near to the charger, and then stop.
# Changed to include docking maneuver attempt
robot.say_text("There you are.").wait_for_completed()
action = robot.go_to_object(
charger,
distance_mm(40.0)) # Distance changed, ALIGNMENT NEEDS WORK!
action.wait_for_completed()
# Following lines added to turn around and dock with charger
robot.turn_in_place(degrees(180)).wait_for_completed()
robot.drive_straight(distance_mm(-150),
speed_mmps(50)).wait_for_completed()
if robot.is_on_charger:
robot.say_text("Home sweet home.").wait_for_completed()
else:
robot.drive_straight(distance_mm(-50),
speed_mmps(50)).wait_for_completed()
if robot.is_on_charger:
robot.say_text("Home sweet home.").wait_for_completed()
def custom_objects(robot: cozmo.robot.Robot):
robot.add_event_handler(cozmo.objects.EvtObjectAppeared,
handle_object_appeared)
# define a unique wall (150mm x 120mm (x10mm thick for all walls)
# with a 50mm x 30mm Circles2 image on front and back
wall_obj = robot.world.define_custom_wall(
CustomObjectTypes.CustomType02,
CustomObjectMarkers.Circles2,
140,
130, #65 by 65 original
20,
20,
False)
wall_obj1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType03,
CustomObjectMarkers.Diamonds2,
140, 130, 20, 20, False)
wall_obj2 = robot.world.define_custom_wall(CustomObjectTypes.CustomType04,
CustomObjectMarkers.Triangles2,
140, 130, 20, 20, False)
wall_obj3 = robot.world.define_custom_wall(CustomObjectTypes.CustomType05,
CustomObjectMarkers.Hexagons2,
140, 130, 20, 20, False)
print(
"Show the above markers to Cozmo and you will see the related objects "
"annotated in Cozmo's view window, you will also see print messages "
"everytime a custom object enters or exits Cozmo's view.")
print("Press CTRL-C to quit")
# Move lift down and tilt the head up
robot.move_lift(100)
# robot.set_head_angle(degrees(-25))#.wait_for_completed()
while True:
robot.set_head_angle(degrees(-16)).wait_for_completed()
cubes = robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.LightCube, timeout=1)
global found_wall
global found_wall1
global found_wall2
global found_wall3
robot.set_head_angle(degrees(-25)).wait_for_completed()
print(found_wall2)
#DIAMOND/RIGHT TURN
if found_wall == True:
robot.drive_straight(distance_mm(130),
speed_mmps(100)).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.set_head_angle(degrees(-25)) #.wait_for_completed()
time.sleep(0.5)
#CIRCLE/LEFT TURN
elif found_wall1 == True:
robot.drive_straight(distance_mm(130),
speed_mmps(100)).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.set_head_angle(degrees(-25)) #.wait_for_completed()
time.sleep(0.5)
#TRIANGLES/STRAIGHT
elif found_wall2 == True:
robot.drive_straight(distance_mm(130),
speed_mmps(100)).wait_for_completed()
robot.set_head_angle(degrees(-25)) #.wait_for_completed()
time.sleep(0.5)
#HEXAGON/TURN AROUND
elif found_wall3 == True:
robot.turn_in_place(degrees(-180)).wait_for_completed()
robot.set_head_angle(degrees(-25)) #.wait_for_completed()
time.sleep(0.5)
#lookaround.stop()
if len(cubes) == 0:
print("Cube not found")
else:
print("Cube found")
if found_wall is False and found_wall is False and found_wall2 is False and found_wall3 is False:
print("Quitting")
break
found_wall = False
found_wall1 = False
found_wall2 = False
found_wall3 = False
time.sleep(0.5)
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)
if robot.is_on_charger:
await robot.drive_off_charger_contacts().wait_for_completed()
await robot.drive_straight(distance_mm(300),
speed_mmps(50)).wait_for_completed()
await robot.set_head_angle(degrees(10)).wait_for_completed()
await robot.set_lift_height(0.0).wait_for_completed()
await robot.say_text('game is on').wait_for_completed()
ball_found = False
try:
while True:
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
ball = find_ball.find_ball(opencv_image,
delta=50,
minCircle=1,
maxCircle=320)
# BallAnnotator.ball = copy.copy(ball)
print('ball: ', ball)
if ball is None:
if not ball_found:
await robot.turn_in_place(degrees(30)).wait_for_completed()
await robot.say_text('searching').wait_for_completed()
elif ball_found:
print('hit the ball: ', ball)
await robot.say_text('Hit it').wait_for_completed()
await robot.drive_straight(
distance_mm(150),
speed_mmps(50),
should_play_anim=False).wait_for_completed()
await robot.set_head_angle(degrees(180)
).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.FistBumpSuccess
).wait_for_completed()
break
else:
robot.stop_all_motors()
if not ball_found:
await robot.say_text('Found it').wait_for_completed()
ball_found = True
h = opencv_image.shape[0]
w = opencv_image.shape[1]
if (ball[2] > 70): # Hit
await robot.say_text('Hit it').wait_for_completed()
await robot.drive_straight(
distance_mm(150),
speed_mmps(50),
should_play_anim=False).wait_for_completed()
await robot.set_head_angle(degrees(180)
).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.FistBumpSuccess
).wait_for_completed()
break
elif (ball[0] > w / 5 * 3): # Right
await robot.turn_in_place(degrees(-5)).wait_for_completed()
elif (ball[0] < w / 5 * 2): # Left
await robot.turn_in_place(degrees(5)).wait_for_completed()
elif (ball[1] > h / 5 * 3): # High
await robot.set_head_angle(
degrees(robot.head_angle.degrees - 5)
).wait_for_completed()
elif (ball[1] < h / 5 * 2): # Low
await robot.set_head_angle(
degrees(robot.head_angle.degrees + 5)
).wait_for_completed()
else:
await robot.drive_straight(
distance_mm(50),
speed_mmps(50),
should_play_anim=False).wait_for_completed()
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print("too many action for cozmo")
print(e)
def cube_return(robot):
# Call functions every time an event happens.
robot.add_event_handler(cozmo.objects.EvtObjectAppeared,
handle_object_appeared)
# Testing variables
# dropoff = robot.world.define_custom_cube(CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons2, 44, 30, 30, True)
# robot.camera.color_image_enabled = True
# Maximum storage distance from the storage position for the cubes.
stordist = 200
# Separation distance between each of the stored cubes.
storstep = 50
# Number of cubes to pick up.
num_cubes = 3
# Check if robot is on charger, our designated starting point.
if robot.is_on_charger == False:
robot.say_text("Off charger.", voice_pitch=-1,
duration_scalar=0.5).wait_for_completed()
# Wait until robot is on charger.
while robot.is_on_charger == False:
time.sleep(1)
# Abbreviate 'robot.world.charger' to 'charger'.
charger = robot.world.charger
# Drive off the charger and save the position for docking back into the charger later.
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(100)).wait_for_completed()
cdpos = copy.deepcopy(robot.pose)
locs.update(
{"charger_dock": [robot.pose.position.x, robot.pose.position.y]})
# Move to the dropping position (predefined) and save this position for later.
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(100)).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
drop = copy.deepcopy(robot.pose)
locs.update({"drop": [robot.pose.position.x, robot.pose.position.y]})
# Go to the scanning point and put the lift and head into their resting positions.
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(100)).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(100)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.set_head_angle(degrees(0.0)).wait_for_completed()
# The list of already found objects.
af = list()
# Finding subroutine
# Repeat the finding subroutine for the cube count.
for i in range(num_cubes):
"""
Run the Find In Place subroutine.
We need to pass 'robot' and 'af' so fip();
can control the robot directly (robot),
and knows what it's already found (af).
"""
findcube, ok = fip(robot, af)
af.append(findcube)
if ok == True:
# If cubes are found, we remember where we were standing last so we can search for the rest.
prev = copy.deepcopy(robot.pose)
locs.update(
{"search_pos": [robot.pose.position.x, robot.pose.position.y]})
# Call the pickup_object subroutine (self-explanatory).
x = robot.pickup_object(findcube,
use_pre_dock_pose=False,
in_parallel=False,
num_retries=5).wait_for_completed()
# Go to the drop off point and drive in using the stor parameters we defined earlier.
robot.go_to_pose(drop).wait_for_completed()
robot.drive_straight(distance_mm(stordist),
speed_mmps(100)).wait_for_completed()
robot.set_lift_height(0.00).wait_for_completed()
robot.drive_straight(distance_mm(-stordist),
speed_mmps(100)).wait_for_completed()
stordist -= storstep
# Return to the previous position.
robot.go_to_pose(prev).wait_for_completed()
async def run(robot: cozmo.robot.Robot):
#disable all annotations from the interface
robot.world.image_annotator.annotation_enabled = False
#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):
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] < 77.5):
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(20)).wait_for_completed()
#cozmo detects cube on right half of screen, reorient itself
elif (BoxAnnotator.cube[0] > 232.5):
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(-20)).wait_for_completed()
#cube is centered, move towards it
elif (BoxAnnotator.cube[0] > 77.5 and BoxAnnotator.cube[0] < 232.5):
#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(20), speed_mmps(20), 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)
def fwd(self, dist):
self.forward(dist)
if self.m_cozmo:
self.m_cozmo.drive_straight(distance_mm(dist), speed_mmps(100))
def go(sdk_conn):
print("COZMO TO GO")
robot = sdk_conn.wait_for_robot()
robot.drive_straight(distance_mm(distance_to_go),
speed_mmps(speed_to_go),
in_parallel=True).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
robot.say_text("Program initializing.").wait_for_completed()
for i in range(2):
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabAmazed).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.turn_in_place(degrees(45)).wait_for_completed()
robot.drive_straight(distance_mm(300), speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabAmazed).wait_for_completed()
robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
for i in range(2):
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabAmazed).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.drive_straight(distance_mm(300), speed_mmps(75)).wait_for_completed()
robot.turn_in_place(degrees(135)).wait_for_completed()
robot.drive_straight(distance_mm(300), speed_mmps(75)).wait_for_completed()
robot.say_text("Song engaged!").wait_for_completed()
# Song Section
notes = [
cozmo.song.SongNote(cozmo.song.NoteTypes.B2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Quarter),
cozmo.song.SongNote(cozmo.song.NoteTypes.B2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.A2,
cozmo.song.NoteDurations.Half),
cozmo.song.SongNote(cozmo.song.NoteTypes.G2,
cozmo.song.NoteDurations.Half)
]
robot.play_song(notes, loop_count=1).wait_for_completed()
robot.say_text("Program shutting down.").wait_for_completed()
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose
global grid, gui, pf
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
camera_settings = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]],
dtype=np.float)
###################
# YOUR CODE HERE
start = time.time()
converged = False
convergence_score = 0
arrived = False
while True:
if arrived:
while not robot.is_picked_up:
await robot.drive_straight(distance_mm(0),
speed_mmps(0)).wait_for_completed()
# Detect whether it is kidnapped
if robot.is_picked_up:
# indicate to re-localize and continue
# print("Picked up")
flag_odom_init = False
arrived = False
converged = False
convergence_score = 0
await robot.drive_wheels(0.0, 0, 0)
# Have the robot act unhappy when we pick it up for kidnapping
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabUnhappy
).wait_for_completed()
while robot.is_picked_up:
await robot.drive_straight(distance_mm(0),
speed_mmps(0)).wait_for_completed()
continue
# use the flag_odom_init to indicate whether it is kidnapped
if flag_odom_init == False:
# Reset the last pose
last_pose = cozmo.util.Pose(0,
0,
0,
angle_z=cozmo.util.Angle(degrees=0))
# Reset particle filter to a uniform distribution
pf.particles = Particle.create_random(PARTICLE_COUNT, grid)
flag_odom_init = True
# Get the current pose
curr_pose = robot.pose
# Obtain odometry information
odom = compute_odometry(curr_pose, cvt_inch=True)
last_pose = robot.pose
# Obtain list of currently seen markers and their poses
marker_list, camera_image = await marker_processing(
robot, camera_settings, show_diagnostic_image=True)
# Update the particle filter using the above information
# Not that the the first element in marker_list is the list
(m_x, m_y, m_h, m_confident) = pf.update(odom, marker_list)
gui.show_particles(pf.particles)
gui.show_mean(m_x, m_y, m_h)
gui.show_camera_image(camera_image)
gui.updated.set()
# print("marker_list:", marker_list)
print(m_x, m_y, m_h, m_confident)
# if converged once
if m_confident:
convergence_score += 3
# Converged many times means good estimate
if convergence_score > 30:
converged = True
# If has converged but diverge again
if converged and not m_confident:
convergence_score -= 2
# if diverge too much
if convergence_score < 0:
converged = False
convergence_score = 0
await robot.drive_wheels(15.0 / (1 + convergence_score / 10),
-15.0 / (1 + convergence_score / 10))
if not converged:
# the localization has not converged -- global localization problem
# Have the robot actively look around (spin in place)
if ((time.time() - start) // 1) % 8 < 3 or len(marker_list) <= 0:
await robot.drive_wheels(15.0, -15, 0)
elif len(marker_list) > 0:
if (marker_list[0][0] > 12):
await robot.drive_wheels(40.0, 40, 0)
if (marker_list[0][0] < 8):
await robot.drive_wheels(-40.0, -40, 0)
else:
await robot.drive_wheels(0.0, 0, 0)
# await robot.drive_wheels(15.0, -15.0)
# if ((time.time() - start) // 1) % 8 == 0 or len(marker_list) <= 0:
# await robot.turn_in_place(degrees(random.randint(-60, 60))).wait_for_completed()
# elif ((time.time() - start) // 1) % 2 == 0 and len(marker_list) > 0:
# await robot.drive_straight(distance_mm(random.randint(-50, 50)), speed_mmps(50)).wait_for_completed()
# else:
# pass
# if ((time.time() - start) // 2) % 3 == 0:
# await robot.drive_wheels(30.0, 30,0)
# elif ((time.time() - start) // 2) % 3 == 1:
# await robot.drive_wheels(-30.0, -30,0)
# elif ((time.time() - start) // 2) % 3 == 2:
# await robot.drive_wheels(-30.0, 30,0)
else:
await robot.drive_wheels(0.0, 0, 0)
dx = goal[0] - m_x
dy = goal[1] - m_y
target_heading = atan2(dy, dx) * 180.0 / PI
dh_deg = diff_heading_deg(target_heading, m_h)
dist = grid_distance(m_x, m_y, goal[0], goal[1])
dh_deg_2 = diff_heading_deg(goal[2], target_heading)
# await robot.turn_in_place(degrees(dh_deg)).wait_for_completed()
await robot.turn_in_place(degrees(int(dh_deg * 0.95))
).wait_for_completed()
# await robot.drive_straight(distance_mm(dist * grid.scale + 1), speed_mmps(50)).wait_for_completed()
await robot.drive_straight(distance_mm(dist * grid.scale * 0.95),
speed_mmps(50)).wait_for_completed()
# await robot.turn_in_place(degrees(dh_deg_2 - sign(dh_deg_2) * 10 )).wait_for_completed()
await robot.turn_in_place(degrees(int(dh_deg_2 * 0.975))
).wait_for_completed()
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabHappy
).wait_for_completed()
arrived = True
robot.say_text("Path one").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabRooster,
in_parallel=True)
robot.drive_straight(distance_mm(100), speed_mmps(50),
should_play_anim=True, in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.say_text("Path two").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabPartyTime,
in_parallel=True)
robot.drive_straight(distance_mm(100), speed_mmps(50))
should_play_anim=True, in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(135)).wait_for_completed()
robot.say_text("Path three").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabScaryCozmo,
in_parallel=True)
robot.drive_straight(distance_mm(141), speed_mmps(50))
should_play_anim=True, in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.say_text("Path four").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabRooster,
in_parallel=True)
robot.drive_straight(distance_mm(100), speed_mmps(50))
should_play_anim=True, in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.say_text("Path five").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabRooster,
in_parallel=True)
robot.drive_straight(distance_mm(100), speed_mmps(50))
should_play_anim=True, in_parallel=True).wait_for_completed()
robot.say_text("Path six").wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabRooster,
def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, confident
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
###################
############YOUR CODE HERE#################
###################
robot.set_head_angle(radians(0)).wait_for_completed()
estimated = [0, 0, 0, False]
trueVal = 0
leaveTurningPhase = False
leaveDriveStraightPhase = False
while True:
# print(robot.is_picked_up)
if risingEdge(flag_odom_init, robot.is_picked_up):
pf = ParticleFilter(grid)
elif robot.is_picked_up:
trueVal = 0
estimated = [0, 0, 0, False]
robot.drive_wheels(0, 0)
elif not robot.is_picked_up:
if trueVal < 30:
robot.drive_wheels(5, 20)
else:
if not leaveTurningPhase:
robot.stop_all_motors()
gh = math.degrees(
math.atan2(goal[1] - estimated[1],
goal[0] - estimated[0]))
ch = estimated[2]
dh = gh - ch
print("gh:", gh % 360, "ch:", ch % 360, "dh:", dh % 360)
robot.turn_in_place(degrees(dh)).wait_for_completed()
leaveTurningPhase = True
print("est h 1", estimated[2])
else:
robot.drive_straight(
distance_inches(
grid_distance(goal[0], goal[1],
estimated[0], estimated[1]) - 1.75),
speed_mmps(25)).wait_for_completed()
print("est h 2", estimated[2])
dh = goal[2] - estimated[2]
print("dh2", dh % 360)
robot.turn_in_place(degrees(dh)).wait_for_completed()
return 0
curr_pose = robot.pose
img = image_processing(robot)
markers = cvt_2Dmarker_measurements(img)
# print(markers)
odom = compute_odometry(curr_pose)
estimated = pf.update(odom, markers)
gui.show_particles(pf.particles)
gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3])
gui.updated.set()
last_pose = curr_pose
if estimated[3]:
trueVal += 1
else:
trueVal = 0
def cozmo_program(robot: cozmo.robot.Robot):
# create an origin point where Cozmo's charger is. When he picks up objects he will return here.
# If the robot was on the charger, drive them forward and clear of the charger
if robot.is_on_charger:
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
robot.move_lift(-3)
robot.turn_in_place(degrees(180)).wait_for_completed()
robot.set_head_angle(degrees(0)).wait_for_completed()
time.sleep(0.5)
# try to find the charger
charger = None
if robot.world.charger:
if robot.world.charger.pose.is_comparable(robot.pose):
# Cozmo knows where the charger is
charger = robot.world.charger
else:
pass
if not charger:
# Tell Cozmo to look around for the charger
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=30)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
look_around.stop()
origin = charger
robot.go_to_object(origin, 70)
# on boot up show loading screen
# locate all cubes
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
look_around.stop()
# define light colours
red_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 0, 0)))
blue_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(0, 0, 255)))
yellow__light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 0)))
# tag each cube found as a different colour
red_cube = robot.world.get_light_cube(LightCube1Id)
blue_cube = robot.world.get_light_cube(LightCube2Id)
yellow_cube = robot.world.get_light_cube(LightCube3Id)
red_cube.set_lights(red_light)
blue_cube.set_lights(blue_light)
yellow_cube.set_lights(yellow__light)
# Pass found objects to GUI for selection
robot.say_text("Ready when you are").wait_for_completed()
# user selects which cube they want
robot.say_text("When you are sure that's the one you want. Press the tick, if you want to select another, "
"just press on another object").wait_for_completed()
# Wait for conformation
robot.say_text("OK, I'll be right back.").wait_for_completed()
# cozmo goes and gets cube
action = robot.go_to_object(red_cube, distance_mm(70.0))
action.wait_for_completed()
action = robot.dock_with_cube(red_cube, approach_angle=cozmo.util.degrees(0), num_retries=2)
action.wait_for_completed()
action = robot.pickup_object(red_cube, num_retries=3)
action.wait_for_completed()
robot.say_text("Got it").wait_for_completed()
# Cozmo returns cube to user
action = robot.go_to_object(origin, 70)
action.wait_for_completed()
robot.say_text("Is this one the right one?").wait_for_completed()
# Object is confirmed
robot.say_text("Yay")
# get dat fist bump
robot.say_text("Do you want me to fetch anything else")
# user says no
# cozmo returns to base
# wait for five minutes of inactivity
robot.say_text("I'm going to have a nap now, let me know if you need anything?")
def run(robot: cozmo.robot.Robot):
max_displays = 500
# get_in_position(robot)
robot.set_head_angle(degrees(5)).wait_for_completed()
# Enable the image stream to receive images
robot.camera.image_stream_enabled = True
# robot.camera.color_image_enabled = True
# This line can be placed inside the loop to ensure only new frames are processed (Slower)
# robot.wait_for(cozmo.world.EvtNewCameraImage)
robot.wait_for(cozmo.world.EvtNewCameraImage)
# robot.drive_wheels(50, 50 )
displays = 0
while displays < 21:
# if (robot.lift_height.distance_mm > 45) or (robot.head_angle.degrees < 10):
# get_in_position(robot)
# Get the raw image from the current frame
raw_image = robot.world.latest_image.raw_image
# Convert to an acceptable format for OpenCV
cv2_image = cv2.cvtColor(numpy.array(raw_image), cv2.COLOR_RGB2BGR)
fileName = 'backRoad' + str(displays)
cv2.imwrite('F:/' + fileName + '.png', cv2_image)
# robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
robot.drive_straight(distance_mm(50), speed_mmps(50),
False).wait_for_completed()
# time.sleep(.5)
# cv2_image = cv2.imread('grayscale.jpg')
# Real Code Goes Here #
# detectShape(cv2_image)
# mask = cv2.inRange(cv2_image, numpy.array([0,0,0]), numpy.array([255, 255, 255]))
# output = cv2.bitwise_and(cv2_image, cv2_image, mask = mask)
# output = cv2.resize(cv2_image, (0,0), fx=.7, fy=.7)
# lower = numpy.array([0, 0, 0])
# upper = numpy.array([230, 230, 230])
# shapeMask = cv2.inRange(output, lower, upper)
# print("I found %d black shapes" % (len(cnts)))
# print("Done")
# cv2.imshow("Mask", shapeMask)
# Example Code #
# Show image on screen for at least 1ms
# cv2.imshow("Window Name", cv2_image)
# cv2.waitKey(1)
# Increment display counter
displays += 1
# Break if display has been shown enough times,
# if displays > max_displays:
# break
robot.stop_all_motors()
def cozmo_program(robot: cozmo.robot.Robot):
success = True
#see what Cozmo sees
robot.camera.image_stream_enabled = True
#connect to cubes (in case Cozmo was disconnected from the cubes)
robot.world.connect_to_cubes()
#identify cubes
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'
if cube1 is not None:
cube1.set_lights(cozmo.lights.red_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")
if cube2 is not None:
cube2.set_lights(cozmo.lights.green_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.")
if cube3 is not None:
cube3.set_lights(cozmo.lights.blue_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube3Id cube - check the battery.")
#have the user tap each of the cubes, in order
try:
robot.say_text("Tap the red cube to make me move.").wait_for_completed()
cube1.wait_for_tap(timeout=10)
except asyncio.TimeoutError:
robot.say_text("The red cube was not tapped").wait_for_completed()
success = False
finally:
cube1.set_lights_off()
if (success):
robot.go_to_object(cube1, distance_mm(20.0)).wait_for_completed()
robot.drive_straight(distance_mm(-100.0), speed_mmps(150)).wait_for_completed()
else:
robot.say_text("You didn't tap the cube properly.").wait_for_completed()
success = True
try:
robot.say_text("Tap the green cube so I can take a picture.").wait_for_completed()
cube2.wait_for_tap(timeout=10)
except asyncio.TimeoutError:
robot.say_text("The green cube was not tapped").wait_for_completed()
success = False
finally:
cube2.set_lights_off()
if (success):
robot.say_text("I see that you were paying attention. I will take a picture.").wait_for_completed()
else:
robot.say_text("Do you know how to tap a cube? I will take a picture anyway.").wait_for_completed()
success = True
new_im = robot.world.wait_for(cozmo.world.EvtNewCameraImage)
new_im.image.raw_image.show()
#save the raw image as a bmp file
img_latest = robot.world.latest_image.raw_image
img_convert = img_latest.convert('L')
img_convert.save("aPhoto.bmp")
#save the raw image data as a png file, named imageName
imageName = "myPhoto.png"
img = Image.open("aPhoto.bmp")
width, height = img.size
new_img = img.resize( (width, height) )
new_img.save( imageName, 'png')
try:
robot.say_text("Tap the blue cube and make me do something.").wait_for_completed()
cube3.wait_for_tap(timeout=10)
except asyncio.TimeoutError:
robot.say_text("The blue cube was not tapped").wait_for_completed()
success = False
finally:
cube3.set_lights_off()
cube = robot.world.wait_for_observed_light_cube()
if (success):
robot.play_anim("anim_cozmosays_badword_01").wait_for_completed()
else:
robot.say_text("Do you know how to tap a cube? I will pop a wheelie.").wait_for_completed()
success = True
action = robot.pop_a_wheelie(cube, num_retries=2)
action.wait_for_completed()
robot.say_text("I treat my body right. Help me.").wait_for_completed()
return