def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
robot.set_head_angle(degrees(0)).wait_for_completed()
while True:
#get camera image
event = robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY)
#find the ball
ball = None
for i in range(10):
ball = find_ball.find_ball(opencv_image)
if ball is not None:
break
if ball is None:
robot.turn_in_place(degrees(10)).wait_for_completed()
continue
#set annotator ball
BallAnnotator.ball = ball
# Turn toward the ball
# If the ball still far away, going toward it
while ball[2] < 130:
robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed()
ball = None
turn = 1
# Adjust the angel to turn toward the ball
while ball is None:
event = 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)
# Do some angle adjustment in case the ball is a bit off to the side
robot.turn_in_place(degrees(turn)).wait_for_completed()
turn = turn * (-2)
time.sleep(0.1)
horizontal_turn = int(float(-ball[0] + len(opencv_image[0])/2) / len(opencv_image[0]) * 25)
robot.turn_in_place(degrees(horizontal_turn)).wait_for_completed()
# Once the ball is big enough, drive a little bit toward it and hit it
robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed()
robot.move_lift(5)
time.sleep(2)
robot.move_lift(-5)
except KeyboardInterrupt:
print("Exit requested by user")
except cozmo.RobotBusy as e:
print('here')
print(e)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
## TODO: ENTER YOUR SOLUTION HERE
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
async def run(self, next_param, robot):
vel = 50
acc = 100
if len(next_param) is not 0:
self.direction = next_param[0]
robot.set_head_angle(cozmo.util.degrees(0), in_parallel=True)
robot.set_lift_height(0, in_parallel=True)
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage,
timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
await robot.drive_wheels(vel * self.direction, -vel * self.direction,
acc, acc)
if ball is not None: #and abs(ball[0] - len(opencv_image[0]) / 2 < 7):
distance = 600 / (ball[2] * 2)
self.found = True
#Have no idea why this works but stops overshot without stopping
await robot.drive_wheels(30, 30, 0, 0)
async def run(self, next_param, robot):
base = 40
mod = .2
acc = 100
robot.set_head_angle(cozmo.util.degrees(-15), in_parallel=True)
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage,
timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
robot.set_lift_height(1, in_parallel=True)
if ball is not None:
self.distance = 600 / (ball[2] * 2)
self.offSet = ball[0] - len(opencv_image[0]) / 2
await robot.drive_wheels(base + self.offSet * mod,
base - self.offSet * mod, acc, acc)
#print(self.distance)
self.search_misses = 0
else:
self.search_misses += 1
self.distance = 9999
if self.distance < 2.5:
robot.set_lift_height(0, in_parallel=True)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
state = 0
def turn_to(curr, goal):
return (curr + goal) / 2
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
goal = [0, 0]
limit = 20
ball_state_weight = 3
thresh = .2 * limit
dist_thresh = event.image.width / 2
if ball is None:
state = max(state - 1, 0)
else:
state = min(state + ball_state_weight, limit)
w = event.image.width
unweighted = [w + ball[0], 2 * w - ball[0]]
goal = np.multiply(unweighted, 30 / (2 * w))
robot.goal = ball
if ball[2] > dist_thresh:
robot.move_lift(5)
else:
robot.move_lift(-5)
if state <= thresh:
motors = np.multiply([10, -10], 1 - (state / thresh))
else:
motors = np.multiply(goal, (state - thresh) / (limit - thresh))
await robot.drive_wheels(*motors)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
found = 0
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
if ball is not None and ball[0] != 0:
found = found + 1
look_around.stop()
b = robot.camera.config.fov_x
amount_to_rotate = radians(b.radians *
(.5 - float(ball[0]) / 320))
await robot.turn_in_place(
amount_to_rotate, in_parallel=True).wait_for_completed()
await robot.drive_straight(
distance_mm(50),
speed_mmps(30),
should_play_anim=False,
in_parallel=True).wait_for_completed()
elif found > 4:
await robot.set_lift_height(
1.0, in_parallel=True).wait_for_completed()
break
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
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)
await robot.set_head_angle(cozmo.util.degrees(10)).wait_for_completed()
print("Press CTRL-C to quit")
try:
isBallDetected = False
while True:
#get camera image
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
# When ball is not seen, turn little, continue until ball is seen
# When ball is seen, move towards the ball, once close to the ball
# (as detected by ball disappearing), stop and move lift up for hurraying :)
if ball is None:
if isBallDetected is True:
break
else:
await robot.turn_in_place(cozmo.util.degrees(10)).wait_for_completed()
else:
isBallDetected = True
await robot.drive_straight(cozmo.util.distance_mm(30), cozmo.util.speed_mmps(30)).wait_for_completed()
robot.move_lift(10)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
#Reset the lift
robot.move_lift(-5)
time.sleep(1)
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
w = opencv_image.shape[1]
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
if ball is None: #If we don't see a ball, turn a little bit and try again
await robot.turn_in_place(degrees(10)).wait_for_completed()
elif (ball[2] < 120
): #If the ball isn't that big yet, keep moving towards it
offset = 0.1 * (w // 2 - ball[0])
await robot.drive_wheels(15 - offset, 15 + offset)
else: #"Hit"the ball
robot.move_lift(5)
time.sleep(1)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
async def image_processing(robot):
global camK, marker_size
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
# convert camera ima ge to opencv format
opencv_image = np.asarray(event.image)
ball = find_ball(opencv_image)
# detect markers
markers = detect_markers(opencv_image, marker_size, camK)
# show markers
for marker in markers:
marker.highlite_marker(opencv_image, draw_frame=True, camK=camK)
#print("ID =", marker.id);
#print(marker.contours);
cv2.imshow("Markers", opencv_image)
return (markers, ball)
def test_single_image(imageNumber, debug=False):
#I am sure there is an easier way to do this, but this is quicker
with open('./imgs/ground_truth.txt') as f:
grid_data = [i.split() for i in f.readlines()]
if len(grid_data) - 1 < imageNumber or imageNumber < 0:
print("image number out of range!")
return -1
fileData = grid_data[imageNumber]
file = fileData[0]
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
#this calls your find_ball and allows you pass your own debug
# a = 0.5
# b = 50
# img = (opencv_image*a)+b
# np.clip(img, 0, 255, out=img)
# img = img.astype('uint8')
ball = find_ball(opencv_image, debug=debug)
display_circles(opencv_image, [ball])
if ball is not None:
print("single image test returned: " + file, ball[0], ball[1], ball[2])
print("expecting: " + fileData[0], fileData[1], fileData[2],
fileData[3])
else:
print("the ball returned is none")
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)
# check each image
bestScore = 0
bestParams = (0,0)
scores = np.zeros((50,50))
for param1value in range(30, 50):
for param2value in range(15, 50):
score = 0
for filedata in grid_data:
file = filedata[0]
#read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
#try to find the ball in the image
ball = find_ball.find_ball(opencv_image, param1value, param2value)
# print(file, ball)
if ball is None:
ball = np.array([0, 0, 0])
# get center err
center_err = math.sqrt((ball[0] - float(filedata[1]))**2 + (
ball[1] - float(filedata[2]))**2)
# get radius err
r_err = math.fabs(ball[2] - float(filedata[3]))
# print("circle center err =", center_err, "pixel")
# print("circle radius err =", r_err, "pixel")
if center_err <= center_err_thresh and r_err <= radius_err_thresh:
async def update_sensors(robot):
"""
Updates the robot's sensors for localization of itself and the ball.
Args:
robot: The robot object.
"""
# Continuously remind Cozmo of camera settings (cause sometimes it
# resets)
robot.camera.set_manual_exposure(10, 3.9)
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
# Convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2BGR)
opencv_image = cv2.bilateralFilter(opencv_image, 10, 75, 50)
# Masks
goal_mask = cv2.inRange(opencv_image, np.array(
[25, 25, 170]), np.array([100, 100, 255]))
opencv_image_hsv = cv2.cvtColor(opencv_image, cv2.COLOR_BGR2HSV)
ball_mask = cv2.inRange(opencv_image_hsv, np.array(
[25, 0, 0]), np.array([100, 255, 255]))
ball_mask = cv2.dilate(ball_mask, None, iterations=1)
# find the ball & goal
ball = find_ball.find_ball(robot, opencv_image_hsv, ball_mask, show_ball)
#ball = (160, 100, 70)
guessed_position, guessed_rotation = find_goal.find_goal(robot, opencv_image, goal_mask, show_goal)
if guessed_position is not None:
grid_guess = robot.grid.worldToGridCoords(guessed_position)
if robot.grid.coordInBounds(grid_guess):
guessed_rotation = math.degrees(-guessed_rotation)
guessed_rotation %= 360
robot.position_queue.put(guessed_position)
if robot.position_queue.qsize() > robot.QUEUE_SIZE:
robot.position_queue.get()
robot.localized = True
robot.position = np.mean(list(robot.position_queue.queue), axis = 0)
robot.rotation_queue.put(guessed_rotation)
if robot.rotation_queue.qsize() > robot.QUEUE_SIZE:
robot.rotation_queue.get()
rotation_vectors = []
for rotation in robot.rotation_queue.queue:
rotation_rad = math.radians(rotation)
rotation_vector = (math.cos(rotation_rad), math.sin(rotation_rad))
rotation_vectors.append(rotation_vector)
average_vector = np.mean(rotation_vectors, axis = 0)
robot.rotation = (math.degrees(math.atan2(*average_vector)) - 90) % 360
else:
robot.position_queue = queue.Queue()
robot.rotation_queue = queue.Queue()
robot.grid_position = robot.grid.worldToGridCoords(robot.position)
robot.prev_grid_position = robot.grid.worldToGridCoords(robot.prev_position)
if gui:
gui.mean_x = robot.grid_position[0]
gui.mean_y = robot.grid_position[1]
gui.mean_heading = robot.rotation
if ball and robot.localized:
radius = ball[2]
x = ball[0]
y = ball[1]
width = len(opencv_image[0])
distance = width / 2 / radius * robot.BALL_RADIUS / robot.TAN_ANGLE + robot.CAMERA_OFFSET
center = width / 2
side_offset = (center - x) / center * robot.ANGLE_OF_VIEW / 2
rotation_rad = math.radians(robot.rotation)
rotation_cos = math.cos(rotation_rad)
rotation_sin = math.sin(rotation_rad)
ball_offset = np.multiply((rotation_cos, rotation_sin), distance)
ball_offset = np.add(ball_offset, np.multiply((rotation_sin, rotation_cos), side_offset))
ball_position = np.add(robot.position, ball_offset)
ball_grid = robot.grid.worldToGridCoords(ball_position)
if robot.grid.coordInBounds(ball_grid):
robot.ball = ball_position
robot.ball_grid = ball_grid
if robot.prev_ball is not None:
robot.prev_ball_grid = robot.grid.worldToGridCoords(
robot.prev_ball)
else:
robot.prev_ball_grid = None
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)
await robot.set_head_angle(degrees(0)).wait_for_completed()
# await robot.set_head_angle(0).wait_for_completed()
textToSay = "One way, or another, I'm going to find you, I'm going to get you get you Mr. Ball"
await robot.say_text(textToSay, voice_pitch=0.8,
duration_scalar=0.5).wait_for_completed()
foundBall = False
try:
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
## TODO: ENTER YOUR SOLUTION HERE
# Determine the w/h of the new image
h = opencv_image.shape[0]
w = opencv_image.shape[1]
if not foundBall and ball is None:
await robot.turn_in_place(degrees(30)).wait_for_completed()
elif foundBall and ball is None:
await robot.drive_wheels(0, 0)
textToSay = "You disappeared Mr. Ball, or should I call you Mr. Houdini!"
await robot.say_text(textToSay,
voice_pitch=0.3,
duration_scalar=0.5).wait_for_completed()
foundBall = False
elif not foundBall and not ball is None:
await robot.drive_wheels(0, 0)
textToSay = "Like I said, I found you Mr. Ball, now I will get you"
await robot.say_text(textToSay,
voice_pitch=0.8,
duration_scalar=0.4).wait_for_completed()
foundBall = True
x = ball[0]
r = ball[2]
if r > 0.7 * w / 2.0:
await robot.drive_wheels(5, 5)
textToSay = "Goodbye Mr. Ball, it was nice knowing you"
await robot.say_text(
textToSay, voice_pitch=0.9,
duration_scalar=0.4).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PounceFace).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PounceInitial
).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PouncePounce).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PounceSuccess
).wait_for_completed()
return
await robot.drive_wheels(50.0 * x / w, 50.0 * (w - x) / w)
else: # foundBall and not ball is None
x = ball[0]
r = ball[2]
if r > 0.7 * w / 2.0:
await robot.drive_wheels(5, 5)
textToSay = "Goodbye Mr. Ball, it was nice knowing you"
await robot.say_text(
textToSay, voice_pitch=0.5,
duration_scalar=0.6).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PounceFace).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PounceInitial
).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PouncePounce).wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.PounceSuccess
).wait_for_completed()
return
await robot.drive_wheels(50.0 * x / w, 50.0 * (w - x) / w)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
state = State()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
trigger = True
isBallFound = False
isRobotAtBall = False
left = False
right = False
robot.set_head_angle(radians(-0.23), in_parallel=True)
# prevPos = None
# direction = None
while trigger:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
h, w = opencv_image.shape
# print(w)
#find the ball
ball = find_ball.find_ball(opencv_image)
distance = calcDistance(ball)
# if prevPos
#set annotator ball
BallAnnotator.ball = ball
BallAnnotator.distance = distance
# BallAnnotator.direction = direction
if state.isCurState("START"):
robot.set_lift_height(0, in_parallel=True)
# robot.display_oled_face_image(image(state.cur), 10.0, in_parallel=True)
#spin around and search for ball
#Make a sound and print something on screen
# display for 1 second
if ball is None:
await robot.drive_wheels(17, -17)
else:
await robot.drive_wheels(0, 0, 0.5)
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
if state.isCurState("TRAVELING"):
# robot.display_oled_face_image(image(state.cur), 10.0, in_parallel=True)
#Print and sound off
# move towards ball
if distance is None:
if left:
lspeed = 5
rspeed = 2 * base
if right:
lspeed = 2 * base
rspeed = 5
await robot.drive_wheels(lspeed, rspeed)
if distance is not None:
if distance > 85:
base = 25
adj = (ball[0] - (w / 2)) / (distance**0.5)
# print(distance)
# print("adj:", adj)
left = adj < -0.75
right = adj > 0.75
if left:
lspeed = base
rspeed = base - adj
# print("LEFT")
elif right:
lspeed = base + adj
rspeed = base
# print("RIGHT")
else:
lspeed = base + 20
rspeed = base + 20
await robot.drive_wheels(lspeed, rspeed)
else:
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
if state.isCurState("END"):
# robot.display_oled_face_image(image(state.cur), 10.0, in_parallel = True)
#tap ball
#Screen and sound off
robot.set_lift_height(1, in_parallel=True).wait_for_completed()
robot.set_lift_height(0, in_parallel=True).wait_for_completed()
if distance is None:
if left or right:
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
elif distance > 85:
state.next()
robot.display_oled_face_image(image(state.cur),
20000,
in_parallel=True)
if state.isCurState("PAUSE"):
#pause for a moment
await robot.drive_wheels(0, 0, 0.05)
state.next()
robot.display_oled_face_image(image(state.cur),
100,
in_parallel=True)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
import find_ball
import math
import numpy as np
# file = 'test91.bmp'
# answer = [160, 56, 13]
file = 'test65.bmp'
answer = [278, 118, 84]
# file = 'test73.bmp'
# answer = [0, 0, 0]
# read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
# try to find the ball in the image
ball = find_ball.find_ball(opencv_image, True, answer)
print(file, ' result: ', ball)
print(file, ' answer: ', answer)
center_err_thresh = 20.0
radius_err_thresh = 10.0
# get center err
center_err = math.sqrt((ball[0] - float(answer[0]))**2 +
(ball[1] - float(answer[1]))**2)
# get radius err
r_err = math.fabs(ball[2] - float(answer[2]))
print("circle center err =", center_err, "pixel")
print("circle radius err =", r_err, "pixel")
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)
currentState = BotState.SEARCHING
cameraFOV = [60, 60]
Kp = [0.25, 0.01]
baseTranslationSpeed = 13
radiusThreshold = 125
try:
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
if (currentState is BotState.SEARCHING):
if (ball is not None):
currentState = BotState.HUNTING
else:
await robot.set_lift_height(1.0).wait_for_completed()
await robot.set_head_angle(cozmo.util.Angle(degrees=0.0)
).wait_for_completed()
await robot.drive_wheels(int(baseTranslationSpeed),
int(-1.0 * baseTranslationSpeed))
elif (currentState is BotState.HUNTING):
if (ball is not None):
imageSizePixels = [320, 240]
errorPixels = [(imageSizePixels[0] / 2) - ball[0],
(imageSizePixels[1] / 2) - ball[1]]
errorNormalizedLinearDegrees = [
(errorPixels[0] /
(imageSizePixels[0] / 2)) * cameraFOV[0],
(errorPixels[1] /
(imageSizePixels[1] / 2)) * cameraFOV[1]
]
proportionalComponent = [
errorNormalizedLinearDegrees[0] * Kp[0],
errorNormalizedLinearDegrees[1] * Kp[1]
]
robot.move_head(proportionalComponent[1])
await robot.drive_wheels(
int(baseTranslationSpeed - proportionalComponent[0]),
int(baseTranslationSpeed + proportionalComponent[0]))
print('cur radius:' + str(ball[2]))
if (ball[2] > radiusThreshold):
robot.stop_all_motors()
currentState = BotState.HITTING
else:
robot.stop_all_motors()
currentState = BotState.SEARCHING
elif (currentState is BotState.HITTING):
await robot.set_lift_height(0.0).wait_for_completed()
await robot.set_lift_height(1.0).wait_for_completed()
currentState = BotState.SEARCHING
print(str(currentState))
## TODO: ENTER YOUR SOLUTION HEREx
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
#print(grid_data)
# thresh hold to accept circle and give credit per circle
center_err_thresh = 20.0
radius_err_thresh = 10.0
score = 0
# check each image
for filedata in grid_data:
file = filedata[0]
#read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
#try to find the ball in the image
ball = find_ball.find_ball(opencv_image)
if ball is None:
ball = np.array([0, 0, 0])
# get center err
center_err = math.sqrt((ball[0] - float(filedata[1]))**2 +
(ball[1] - float(filedata[2]))**2)
# get radius err
r_err = math.fabs(ball[2] - float(filedata[3]))
if center_err <= center_err_thresh and r_err <= radius_err_thresh:
score += 1
else:
print(file, ball)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
stopped = False
count = 0
await robot.set_lift_height(0).wait_for_completed()
await robot.set_head_angle(degrees(0)).wait_for_completed()
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
## TODO: ENTER YOUR SOLUTION HERE
#we will say we are near the ball when ball is over 50% of the screen
if stopped is True:
await robot.say_text("I found the ball").wait_for_completed()
await robot.drive_wheels(20, 20)
time.sleep(2.5)
await robot.drive_wheels(0, 0)
await robot.set_lift_height(0.75).wait_for_completed()
await robot.set_lift_height(0).wait_for_completed()
return
else:
if ball is not None:
area = ball[2] * ball[2] * 3.14
height, width = opencv_image.shape
ratio = (area / (height * width)) * 100
print(ratio)
if ratio > 35: #we are close to ball
motor_left = 0
motor_right = 0
stopped = True
else:
ballcenter = ball[0]
motor_right = (width - ballcenter) / 4
motor_left = ballcenter / 4
else:
motor_right = 30
motor_left = 40
print("motor_right: " + str(motor_right))
print("motor_left: " + str(motor_left))
# Send commands to the robot
await robot.drive_wheels(motor_left, motor_right)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
import cv2
import find_ball
file = '9.png'
# read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
# try to find the ball in the image
ball = find_ball.find_ball(opencv_image,
True,
delta=50,
minCircle=1,
maxCircle=320)
print(file, ' ball: ', ball)
print(grid_data)
# thresh hold to accept circle and give credit per circle
center_err_thresh = 20.0
radius_err_thresh = 10.0
score = 0;
# check each image
for filedata in grid_data:
file = filedata[0]
#read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
#try to find the ball in the image
ball = find_ball.find_ball(opencv_image, False)
print(file, ball)
if ball is None:
ball = np.array([0, 0, 0])
# get center err
center_err = math.sqrt((ball[0] - float(filedata[1]))**2 + (
ball[1] - float(filedata[2]))**2)
# get radius err
r_err = math.fabs(ball[2] - float(filedata[3]))
print("circle center err =", center_err, "pixel")
print("circle radius err =", r_err, "pixel")
if center_err <= center_err_thresh and r_err <= radius_err_thresh:
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
looking_around = None
robot.set_lift_height(0)
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
h = opencv_image.shape[0]
w = opencv_image.shape[1]
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
if np.array_equal(ball, [0, 0, 0]):
if not looking_around:
robot.stop_all_motors()
looking_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
else:
# Stop moving around
if looking_around:
looking_around.stop()
looking_around = None
# If reached the ball (by checking the visible radius), lift it
if ball[2] > 95:
robot.stop_all_motors()
# Drive the final distance in a straight line (we'll lose sight of the ball when it's too close)
await robot.drive_straight(
cozmo.util.distance_mm(30),
cozmo.util.speed_mmps(10)).wait_for_completed()
await robot.set_lift_height(1.0).wait_for_completed()
return
# Move head to center the ball along the y axis
# Ball on the top -> head_speed = 0.5
# Ball on the bottom -> head_speed = -0.5
head_speed = 0.5 - ball[1] / h
print("Ball = {0}, Head speed = {1}".format(ball, head_speed))
robot.move_head(head_speed)
# Drive towards the ball like a Braitenberg vehicle.
# Larger radius -> slower
speed = 300 / ball[2]
# Ball on the left -> diff = -0.5
# Ball on the right -> diff = 0.5
diff = ball[0] / w - 0.5
left_speed = speed * (1 + diff)
right_speed = speed * (1 - diff)
print(
"Speed = {0}, Diff = {1}, Left = {2}, Right = {3}".format(
speed, diff, left_speed, right_speed))
robot.drive_wheel_motors(left_speed, right_speed)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
import sys
import numpy as np
print("start.....")
for guessDP in range(100, 176, 5):
for delta in range(64, 44, -2):
score = 0
for index in range(1, 11):
file = f'{index}.png'
# read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
# try to find the ball in the image
ball = find_ball.find_ball(opencv_image,
False,
delta=delta,
guessDP=guessDP,
minCircle=15,
maxCircle=320)
if (index < 5 and ball is None):
score += 1
elif (index > 4 and ball is not None):
score += 1
print(
f"score = {score}, guessDP = {float(guessDP)/100}, delta = {delta}"
)
grid_data = [i.split() for i in f.readlines()]
# thresh hold to accept circle and give credit per circle
center_err_thresh = 20.0
radius_err_thresh = 10.0
score = 0;
# check each image
for filedata in grid_data:
file = filedata[0]
#read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
#try to find the ball in the image
ball = find_ball.find_ball(opencv_image, debug=True)
expected_ball = list(map(int, filedata[1:3]))
if ball is None:
ball = np.array([0, 0, 0])
# get center err
center_err = math.sqrt((ball[0] - float(filedata[1]))**2 + (
ball[1] - float(filedata[2]))**2)
# get radius err
r_err = math.fabs(ball[2] - float(filedata[3]))
correct = center_err <= center_err_thresh and r_err <= radius_err_thresh
print("%s: %s |%s - %s| = %.1f , |%s - %s| = %.1f" % (
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
# add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
await robot.set_lift_height(0.0, in_parallel=True).wait_for_completed()
await robot.set_head_angle(degrees(-17), in_parallel=True).wait_for_completed()
focalLength = 132.7
actualBallHeight = 40
ballDetected = 0
ballCentered = 0
isNotFinished = 1
state = "initial"
prevBall = [0, 0, 0]
distanceCheck = 3
prevDistance = 0
while isNotFinished:
print(state)
# get camera image
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
# convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY)
height, width = opencv_image.shape[:2]
# look for ball
ball = find_ball.find_ball(opencv_image)
if ball[0] != 0 or ball[1] != 0 or ball[2] != 0:
ballDetected = 1
ballCentered = isCentered(ball, width)
prevBall = ball
else:
ball = prevBall
# ballDetected = 0
# ballCentered = 0
BallAnnotator.ball = ball
if state == "initial":
if ballDetected == 1:
await robot.say_text("I found the ball. ", in_parallel=True).wait_for_completed()
if ballCentered:
state = "drive"
else:
state = "center"
else:
state = "search"
elif state == "search":
robot.stop_all_motors()
if ballDetected == 1:
await robot.say_text("I found the ball. ", in_parallel=True).wait_for_completed()
if ballCentered:
state = "drive"
else:
state = "center"
else:
await robot.turn_in_place(degrees(-35), in_parallel=False, num_retries=0).wait_for_completed()
elif state == "center":
robot.stop_all_motors()
xCenter = width / 2
ballX = ball[0]
if ballDetected == 0:
state = "search"
else:
if ballCentered:
state = "drive"
else:
dist = xCenter - ballX
angle = (67.0/320.0 * dist) - 2
if ballX < xCenter:
await robot.turn_in_place(degrees(angle), in_parallel=False, num_retries=0).wait_for_completed()
elif ballX > xCenter:
await robot.turn_in_place(degrees(angle), in_parallel=False, num_retries=0).wait_for_completed()
elif state == "drive":
if ballDetected:
distance = actualBallHeight * focalLength / ball[2]
if abs(prevDistance - distance) < 2 and distance < 90:
print("didn't move")
if distanceCheck == 0:
state = "hit ball"
else:
distanceCheck -= 1
else:
distanceCheck = 3
prevDistance = distance
print("distance: ", distance, "radius: ", ball[2])
if distance < 56 or ball[2] > 94:
state = "hit ball"
else:
if ballDetected == 0:
robot.stop_all_motors()
state = "search"
else:
if ballCentered == 0:
robot.stop_all_motors()
state = "center"
else:
await robot.drive_wheels(85, 85)
else:
state = "center"
elif state == "hit ball":
robot.stop_all_motors()
await robot.say_text("I hit the ball. ", in_parallel=True).wait_for_completed()
await robot.set_lift_height(0.95, in_parallel=True).wait_for_completed()
isNotFinished = 0
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
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)
robot.move_lift(-3)
robot.set_head_angle(cozmo.util.degrees(-10)).wait_for_completed()
try:
while True:
#get camera image
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY)
height, width = opencv_image.shape
center = width/2.0
threshold = 60 # threshold for the center of the image
#find the ball
ball = find_ball.find_ball(opencv_image, False)
x = ball[0]
y = ball[1]
radius = ball[2]
#set annotator ball
BallAnnotator.ball = ball
motor_left = motor_right = 0
# spin in a circle until a ball is found
if x == 0 and y == 0 and radius == 0:
motor_left = -10
motor_right = 10
# hit the ball when we're close enough
elif radius > 115:
await robot.drive_wheels(0, 0)
time.sleep(1.0)
robot.move_lift(1)
time.sleep(5.0)
break
elif abs(x-center) < threshold:
motor_left = 10
motor_right = 10
elif x > center:
motor_left = 10
elif x < center:
motor_right = 10
await robot.drive_wheels(motor_left, motor_right)
time.sleep(.1)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
# add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
await robot.set_lift_height(0.0, in_parallel=True).wait_for_completed()
await robot.set_head_angle(degrees(-17),
in_parallel=True).wait_for_completed()
focalLength = 132.7
actualBallHeight = 40
isNotFinished = 1
state = "initial"
prevBall = [0, 0, 0]
robot.set_robot_volume(0.3)
myCozmo = cozmoFSM("myCozmo")
while isNotFinished:
oled_face_data = cozmo.oled_face.convert_image_to_screen_data(
make_text_image(myCozmo.state, 8, 6, _clock_font))
robot.display_oled_face_image(oled_face_data,
1000.0,
in_parallel=True)
# get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
# convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
height, width = opencv_image.shape[:2]
# look for ball
ball = find_ball.find_ball(opencv_image)
if ball[0] != 0 or ball[1] != 0 or ball[2] != 0:
ballDetected = 1
prevBall = ball
else:
ball = None
ballDetected = 0
BallAnnotator.ball = ball
if myCozmo.state == "initial":
print("Starting state: initial")
# print('\a')
if ballDetected == 1:
print("Previous state: initial, new state: drive")
myCozmo.ballDetected()
else:
print("Previous state: initial, new state: search")
myCozmo.ballLost()
# print('\a')
elif myCozmo.state == "search":
# robot.stop_all_motors()
if ballDetected == 1:
robot.stop_all_motors()
print("Previous state: search, new state: drive")
myCozmo.ballDetected()
# print('\a')
continue
else:
if prevBall[0] != 0 or prevBall[1] != 0 or prevBall[2] != 0:
offset = width / 2 - prevBall[0]
if offset < 0:
await robot.drive_wheels(35, -35)
else:
await robot.drive_wheels(-35, 35)
else:
await robot.drive_wheels(-35, 35)
elif myCozmo.state == "drive":
if not ballDetected:
print("Previous state: drive, new state: search")
myCozmo.ballLost()
# print('\a')
continue
if ballDetected:
distance = actualBallHeight * focalLength / ball[2]
leftConst = 70
rightConst = 70
offset = width / 2 - ball[0]
leftSpeed = leftConst - 0.333 * offset
rightSpeed = rightConst + 0.333 * offset
if distance < 54 or ball[2] > 98:
print("Previous state: drive, new state: hit ball")
myCozmo.ballInReach()
# print(state)
# print('\a')
else:
await robot.drive_wheels(leftSpeed, rightSpeed)
elif myCozmo.state == "hitball":
await robot.set_lift_height(
0.5, in_parallel=True).wait_for_completed()
await robot.set_lift_height(
0.1, in_parallel=True).wait_for_completed()
print("Previous state: hit ball, new state: drive")
myCozmo.ballDetected()
# print('\a')
# isNotFinished = 0
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
img = cv2.cvtColor(
np.array(
pyautogui.screenshot('screen.png',
region=(border[0], border[1],
game_res[0] + border[0],
game_res[1] + border[1]))),
cv2.COLOR_BGR2GRAY)
# cut out the bottom middle third of the screen as ROI for the ball
ball_roi = img[int(img.shape[0] / 2):img.shape[0],
int(img.shape[1] * 1 / 3):int(img.shape[1] * 2 / 3)]
# insert the ball ROI back into the full image
img[int(img.shape[0] / 2):img.shape[0],
int(img.shape[1] * 1 / 3):int(img.shape[1] * 2 /
3)] = find_ball(ball_roi)
# cut out the bottom middle third of the screen as ROI for the targeting line
targeting_line_roi = img[0:img.shape[0],
int(img.shape[1] * 1 / 3):int(img.shape[1] *
2 / 3)]
# insert the targeting line ROI back into the full image
img[0:img.shape[0],
int(img.shape[1] * 1 /
3):int(img.shape[1] * 2 /
3)] = find_targeting_line(targeting_line_roi)
cv2.imshow('image', img)
if cv2.waitKey(25) & 0xFF == ord('q'):
import cv2
import find_ball
for index in range(1, 14):
file = f'{index}.png'
# read in image as grayscale
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
# try to find the ball in the image
ball = find_ball.find_ball(opencv_image,
False,
delta=50,
minCircle=5,
maxCircle=320)
print(file, ' ball: ', ball)
score = 0
print("start.....")
for guessDP in range(140, 176, 5):
for delta in range(62, 44, -2):
score = 0
for filedata in grid_data:
file = filedata[0]
opencv_image = cv2.imread("./imgs/" + file, cv2.COLOR_GRAY2RGB)
ball = find_ball.find_ball(
opencv_image,
False, [int(filedata[1]),
int(filedata[2]),
int(filedata[3])],
delta=delta,
guessDP=float(guessDP) / 100)
if ball is None:
ball = np.array([0, 0, 0])
center_err = math.sqrt((ball[0] - float(filedata[1]))**2 +
(ball[1] - float(filedata[2]))**2)
r_err = math.fabs(ball[2] - float(filedata[3]))
if center_err <= center_err_thresh and r_err <= radius_err_thresh:
score += 1
print(
f"score = {score}, guessDP = {float(guessDP)/100}, delta = {delta}"
)
