def pycozmo_program(cli: pycozmo.client.Client):
global last_im
# Raise head.
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians -
pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
cli.set_head_angle(angle)
# Register to receive new camera images.
cli.add_handler(pycozmo.event.EvtNewRawCameraImage, on_camera_image)
# Enable camera.
pkt = pycozmo.protocol_encoder.EnableCamera()
cli.conn.send(pkt)
while True:
if last_im:
# Get last image.
im = last_im
# Resize from 320x240 to 128x32.
im = im.resize((128, 32))
# Convert to binary image.
im = im.convert('1')
# Display the result image.
cli.display_image(im)
# Run with 25 FPS.
time.sleep(1 / 25)
def pycozmo_program(cli: pycozmo.client.Client):
# spin up the Frame Analysis stuff
frame_analysis = FrameAnalysis()
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians -
pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
cli.set_head_angle(angle)
pkt = pycozmo.protocol_encoder.EnableCamera()
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.EnableColorImages(enable=True)
cli.conn.send(pkt)
# Wait for image to stabilize.
time.sleep(2.0)
# handler to get image and feed to frame analysis
cli.add_handler(pycozmo.event.EvtNewRawCameraImage,
frame_analysis.new_image,
one_shot=True)
# Wait for image to be captured.
time.sleep(1)
# Ouput frame and edges
plt.subplot(121), plt.imshow(frame_analysis.image, cmap='gray')
plt.title('Original'), plt.xticks([]), plt.yticks([])
plt.subplot(122), plt.imshow(frame_analysis.edges, cmap='gray')
plt.title('Edge'), plt.xticks([]), plt.yticks([])
plt.show()
def pycozmo_program(cli: pycozmo.client.Client):
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians - pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
cli.set_head_angle(angle)
time.sleep(1)
# Load image
im = Image.open(os.path.join(os.path.dirname(__file__), "..", "assets", "pycozmo.png"))
# Convert to binary image.
im = im.convert('1')
cli.display_image(im, 10)
def pycozmo_program(cli: pycozmo.client.Client):
cli.conn.add_handler(pycozmo.protocol_encoder.RobotState, on_robot_state, one_shot=True)
cli.conn.add_handler(pycozmo.protocol_encoder.RobotPoked, on_robot_poked)
cli.conn.add_handler(pycozmo.protocol_encoder.FallingStarted, on_robot_falling_started)
cli.conn.add_handler(pycozmo.protocol_encoder.FallingStopped, on_robot_falling_stopped)
cli.conn.add_handler(pycozmo.protocol_encoder.ButtonPressed, on_button_pressed)
cli.add_handler(pycozmo.event.EvtRobotPickedUpChange, on_robot_picked_up)
cli.add_handler(pycozmo.event.EvtRobotChargingChange, on_robot_charging)
cli.add_handler(pycozmo.event.EvtCliffDetectedChange, on_cliff_detected)
cli.add_handler(pycozmo.event.EvtRobotWheelsMovingChange, on_robot_wheels_moving)
heart = Image.open("hjarta.png")
heart = heart.convert('1')
eyes = Image.open("eyes.png")
eyes = eyes.convert('1')
cli.display_image(eyes)
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians - pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
#cli.set_head_angle(angle)
cli.set_head_angle(0)
pkt = pycozmo.protocol_encoder.EnableCamera(enable=True)
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.EnableColorImages(enable=True)
cli.conn.send(pkt)
# Wait for image to stabilize.
time.sleep(1.0)
cli.add_handler(pycozmo.event.EvtNewRawCameraImage, on_camera_image, one_shot=False)
speed = 100
cli.drive_wheels(lwheel_speed=-speed,rwheel_speed=-speed, duration=0.5)
cli.drive_wheels(lwheel_speed=speed,rwheel_speed=speed, duration=1.0)
cli.drive_wheels(lwheel_speed=speed,rwheel_speed=speed, duration=3.0)
time.sleep(1)
cli.display_image(heart)
time.sleep(1)
cli.display_image(eyes)
turnduration=1.38
cli.drive_wheels(lwheel_speed=speed,rwheel_speed=-speed, duration=turnduration)
cli.drive_wheels(lwheel_speed=speed,rwheel_speed=speed, duration=2.25)
time.sleep(1)
cli.drive_wheels(lwheel_speed=speed,rwheel_speed=-speed, duration=turnduration)
cli.display_image(heart)
cli.drive_wheels(lwheel_speed=-speed,rwheel_speed=-speed, duration=1.7)
cli.display_image(eyes)
time.sleep(1)
def pycozmo_program(cli: pycozmo.client.Client):
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians -
pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
cli.set_head_angle(angle)
time.sleep(1)
# Render a 128x64 procedural face with default parameters.
f = pycozmo.procedural_face.ProceduralFace()
im = f.render()
# The Cozmo protocol expects a 128x32 image, so take only the even lines.
np_im = np.array(im)
np_im2 = np_im[::2]
im2 = Image.fromarray(np_im2)
cli.display_image(im2, 10)
def pycozmo_program(cli: pycozmo.client.Client):
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians -
pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
cli.set_head_angle(angle)
time.sleep(1)
# Generate random dots.
dots = []
for i in range(NUM_DOTS):
x = random.randint(0, WIDTH)
y = random.randint(0, HEIGHT)
vx = random.randint(-MAX_SPEED, MAX_SPEED)
vy = random.randint(-MAX_SPEED, MAX_SPEED)
dot = Dot(x, y, vx, vy)
dots.append(dot)
while True:
# Create a blank image.
im = Image.new("1", (128, 32), color=0)
# Draw lines.
draw = ImageDraw.Draw(im)
for a, b in itertools.combinations(dots, 2):
draw.line((a.x, a.y, b.x, b.y), width=LINE_WIDTH, fill=1)
# Move dots.
for dot in dots:
dot.x += dot.vx
dot.y += dot.vy
if dot.x <= DOT_SIZE:
dot.x = DOT_SIZE
dot.vx = abs(dot.vx)
elif dot.x >= WIDTH - DOT_SIZE:
dot.x = WIDTH - DOT_SIZE
dot.vx = -abs(dot.vx)
if dot.y <= DOT_SIZE:
dot.y = DOT_SIZE
dot.vy = abs(dot.vy)
elif dot.y >= HEIGHT - DOT_SIZE:
dot.y = HEIGHT - DOT_SIZE
dot.vy = -abs(dot.vy)
cli.display_image(im, DELAY)
def pycozmo_program(cli: pycozmo.client.Client):
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians -
pycozmo.robot.MIN_HEAD_ANGLE.radians) / 2.0
cli.set_head_angle(angle)
pkt = pycozmo.protocol_encoder.EnableCamera()
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.EnableColorImages(enable=True)
cli.conn.send(pkt)
# Wait for image to stabilize.
time.sleep(2.0)
cli.add_handler(pycozmo.event.EvtNewRawCameraImage,
on_camera_image,
one_shot=True)
# Wait for image to be captured.
time.sleep(1)
def pycozmo_program(cli: pycozmo.client.Client):
global last_im, updated
# activate the pygame library .
# initiate pygame and give permission
# to use pygame's functionality.
pygame.init()
# define the RGB value
# for white colour
white = (255, 255, 255)
# assigning values to X and Y variable
X = 320
Y = 720
# create the display surface object
# of specific dimension..e(X, Y).
display_surface = pygame.display.set_mode((X, Y ))
# set the pygame window name
pygame.display.set_caption('Image')
# Set to look straight ahead
cli.set_head_angle(0)
pkt = pycozmo.protocol_encoder.EnableCamera()
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.EnableColorImages(enable=True)
cli.conn.send(pkt)
# Wait for image to stabilize.
time.sleep(2.0)
# Register to receive new camera images.
cli.add_handler(pycozmo.event.EvtNewRawCameraImage, on_camera_image)
# MANUAL DISTANCE CALIBRATIONS
calibrated = False
# calibration: distance from cozmo to object (inches)
KNOWN_DISTANCE = 12.0
# calibration: width of your object of interest (inches)
KNOWN_WIDTH = 1.0
fgbg = cv.createBackgroundSubtractorMOG2(
history=10,
varThreshold=2,
detectShadows=False)
while True:
if updated:
# Get last image.
im = last_im
im2 = im.copy()
gray = cv.cvtColor(im2, cv.COLOR_BGR2GRAY)
updated = False
# filtering https://www.sicara.ai/blog/2019-03-12-edge-detection-in-opencv
filtered = cv.bilateralFilter(gray, 7, 50, 50)
foreground = fgbg.apply(filtered)
kernel = np.ones((50,50),np.uint8)
foreground = cv.morphologyEx(foreground, cv.MORPH_CLOSE, kernel)
# edge detection
edges = cv.Canny(filtered,100,200)
# Crop off moving area
cropped = (foreground //255) * edges
# DISTANCE DETECTION
# referenced https://www.pyimagesearch.com/2015/01/19/find-distance-camera-objectmarker-using-python-opencv/
# create a list of contours to work with
boxContours = cv.findContours(edges.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
boxContours = imutils.grab_contours(boxContours)
# try finding the contour with the largest area
try:
# using the boxContours list, finds biggest in terms of area, fits rect to that area
target = max(boxContours, key = cv.contourArea)
targetBox = cv.minAreaRect(target)
# get a calibrated focal length for cozmo camera, first pass only
if calibrated == False:
focalLength = (targetBox[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
calibrated = True
# find distance to object based on:
# manually set KNOWN_WIDTH (earlier in code, starting contour width)
# focalLength (calculated on the first pass, scale factor)
# targetBox[1][0], which is this frame's contour width
# scale factor * (starting width / current width)
currentDistance = focalLength * (KNOWN_WIDTH / targetBox[1][0])
# draw bounding box
box = cv.boxPoints(targetBox)
box = np.int0(box)
boxImg = cv.drawContours(im2, [box], -1, (255, 255, 150), 2)
# add text below the bounding box
cv.putText(boxImg, "%.2fin" % (currentDistance),
(boxImg.shape[1] - 200, boxImg.shape[0] - 20),
cv.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 150), 2)
# if no contours are available
except ValueError:
print("No object detected")
# completely fill the surface object
# with white colour
display_surface.fill(white)
# copying the image surface object
# to the display surface object at
# (0, 0) coordinate.
display_surface.blit(cv2ImageToSurface(im), (0, 0))
display_surface.blit(cv2ImageToSurface(cropped), (0,240))
# draw the distance detecting image
display_surface.blit(cv2ImageToSurface(boxImg), (0, 480))
# Draws the surface object to the screen.
pygame.display.update()
# iterate over the list of Event objects
# that was returned by pygame.event.get() method.
for event in pygame.event.get() :
# if event object type is QUIT
# then quitting the pygame
# and program both.
if event.type == pygame.QUIT :
# deactivates the pygame library
pygame.quit()
# quit the program.
quit()
# Run with 25 FPS.
time.sleep(1 / 25)
def pycozmo_program(cli: pycozmo.client.Client):
global last_im, updated
yolo = YOLO("./yolo-coco/coco.names", "./yolo-coco/yolov3.weights",
"./yolo-coco/yolov3.cfg", 0.5, 0.3)
# activate the pygame library .
# initiate pygame and give permission
# to use pygame's functionality.
pygame.init()
# define the RGB value
# for white colour
white = (255, 255, 255)
# assigning values to X and Y variable
X = 320
Y = 480
# create the display surface object
# of specific dimension..e(X, Y).
display_surface = pygame.display.set_mode((X, Y))
# set the pygame window name
pygame.display.set_caption('Image')
# Raise head.
angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians -
pycozmo.robot.MIN_HEAD_ANGLE.radians) / 4.0
cli.set_head_angle(angle)
pkt = pycozmo.protocol_encoder.EnableCamera()
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.EnableColorImages(enable=True)
cli.conn.send(pkt)
# Wait for image to stabilize.
time.sleep(2.0)
# Register to receive new camera images.
cli.add_handler(pycozmo.event.EvtNewRawCameraImage, on_camera_image)
while True:
if updated:
# Get last image.
#im = last_im.copy()
updated = False
#cv2.imshow('Frame',im)
detect_im = yolo.analyze_image(last_im.copy())
#cv2.imshow('Found',detect_im)
im_both = np.vstack((last_im, detect_im))
# completely fill the surface object
# with white colour
display_surface.fill(white)
# copying the image surface object
# to the display surface object at
# (0, 0) coordinate.
display_surface.blit(cv2ImageToSurface(im_both), (0, 0))
#display_surface.blit(cv2ImageToSurface(im), (0,240))
# Draws the surface object to the screen.
pygame.display.update()
# iterate over the list of Event objects
# that was returned by pygame.event.get() method.
for event in pygame.event.get():
# if event object type is QUIT
# then quitting the pygame
# and program both.
if event.type == pygame.QUIT:
# deactivates the pygame library
pygame.quit()
# quit the program.
quit()
# Run with 25 FPS.
time.sleep(1 / 25)
