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):
cli.conn.add_handler(pycozmo.protocol_encoder.PathFollowingEvent,
on_path_following_event)
cli.add_handler(pycozmo.event.EvtRobotPathingChange,
on_robot_pathing_change)
pkt = pycozmo.protocol_encoder.AppendPathSegLine(from_x=0.0,
from_y=0.0,
to_x=150.0,
to_y=0.0,
speed_mmps=SPEED_MMPS,
accel_mmps2=ACCEL_MMPS2,
decel_mmps2=DECEL_MMPS2)
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.AppendPathSegLine(from_x=150.0,
from_y=0.0,
to_x=150.0,
to_y=150.0,
speed_mmps=SPEED_MMPS,
accel_mmps2=ACCEL_MMPS2,
decel_mmps2=DECEL_MMPS2)
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.AppendPathSegLine(from_x=150.0,
from_y=150.0,
to_x=0.0,
to_y=150.0,
speed_mmps=SPEED_MMPS,
accel_mmps2=ACCEL_MMPS2,
decel_mmps2=DECEL_MMPS2)
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.AppendPathSegLine(from_x=0.0,
from_y=150.0,
to_x=0.0,
to_y=0.0,
speed_mmps=SPEED_MMPS,
accel_mmps2=ACCEL_MMPS2,
decel_mmps2=DECEL_MMPS2)
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.AppendPathSegPointTurn(
x=0.0,
y=0.0,
angle_rad=pycozmo.util.Angle(degrees=0.0).radians,
angle_tolerance_rad=0.01,
speed_mmps=SPEED_MMPS,
accel_mmps2=ACCEL_MMPS2,
decel_mmps2=DECEL_MMPS2)
cli.conn.send(pkt)
pkt = pycozmo.protocol_encoder.ExecutePath(event_id=1)
cli.conn.send(pkt)
e.wait(timeout=30.0)
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):
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 update(cli: pycozmo.client.Client) -> None:
""" Perform robot OTA firmware update. """
# Register for FirmwareUpdateResult packets.
cli.add_handler(pycozmo.protocol_encoder.FirmwareUpdateResult, on_firmware_update_result)
safe_size = os.path.getsize(safe_file)
total_chunks = math.ceil(safe_size / 1024)
if verbose:
print("Safe size: {} bytes / {} chunks".format(safe_size, total_chunks))
with open(safe_file, "rb") as f:
print("Initiating update...")
send_chunk(cli, f)
wait_for_result(30.0)
if last_status != 0:
raise UpdateError("Failed to receive initialization confirmation.")
print("Transferring firmware...")
done = False
while not done:
if verbose:
print("Sending chunk {}/{}...".format(chunk_id+1, total_chunks))
# For some reason the robot sends FirmwareUpdateResult only every 2 chunks.
send_chunk(cli, f)
done = send_chunk(cli, f)
wait_for_result(0.5)
# Finalize update
print("Finalizing update...")
pkt = pycozmo.protocol_encoder.FirmwareUpdate(chunk_id=0xFFFF, data=b"\0" * 1024)
cli.conn.send(pkt)
wait_for_result(10.0)
if last_status != 10:
raise UpdateError("Failed to receive update confirmation (status {}).".format(last_status))
time.sleep(15.0)
print("Done.")
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)
while True:
try:
time.sleep(0.1)
except KeyboardInterrupt:
break
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)