def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment description for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
#set/import initial grid conditions, initialize variables. set a default initial goal to be the center of the map.
botPoint = (3, 2)
grid.setStart(botPoint)
grid.addGoal((13, 9))
cubes = set()
goalCubePos = None
goalScalar = -1
cubeFootprint = [-2, -1, 0, 1, 2]
scale = 25
width = 26
height = 18
repath = False
#reset the robot for its journey
robot.image_stream_enabled = True
robot.set_head_angle(degrees(0)).wait_for_completed()
robot.move_lift(-3)
speed = 27
previousHeading = 0
while not stopevent.is_set():
#if repath == True:
grid.clearStart()
grid.setStart(botPoint)
# repath = False;
#run astar search
astar(grid, heuristic)
path = grid.getPath()
#where are we now?
if len(path) - path.index(botPoint) == 1:
endPoint = botPoint
else:
endPoint = path[path.index(botPoint) + 1]
print("cur: {0}".format(botPoint))
print("end: {0}".format(endPoint))
#find the cube(s)
for cube in robot.world.visible_objects:
cubeID = cube.object_id
#if this is a 'new' cube...
if cubeID not in cubes:
cubex = cube.pose.position.x
cubey = cube.pose.position.y
#normalize the position to the size of the grid and make sure that any cubes are located INSIDE the grid
cubex = min(math.ceil((cubex) / scale) + 2, width)
cubey = min(math.ceil((cubey) / scale) + 2, height)
#mark grid squares as obstacles, based on the normalized cube position and the footprint of the cube
for xi in cubeFootprint:
for yi in cubeFootprint:
curx = cubex + xi
cury = cubey + yi
#only mark a grid square as containing an obstacle if the square is actually inside the grid
if curx >= 0 and curx <= width and cury >= 0 and cury <= height:
grid.addObstacle((curx, cury))
grid.clearVisited()
grid.setStart(endPoint)
print("Found a cube: {0}".format(cubeID))
#add this cube to the master set of cubes
cubes.add(cubeID)
#special casing if we realize that this is the goal cube
#if robot.world.light_cubes[cozmo.objects.LightCube1Id].object_id == cubeID:
if cubeID == 1:
goalCubePos = (cubex, cubey)
#adjust the bot's final position based on the rotational position of the cube so that the bot can approach it properly
theta = cube.pose.rotation.angle_z.radians + math.pi
goalx = cubex + (round(math.cos(theta)) *
math.floor((len(cubeFootprint) / 2) + 1))
goaly = cubey + (round(math.sin(theta)) *
math.floor((len(cubeFootprint) / 2) + 1))
#get rid of any stale goals (e.g. center of the grid) and add the goal position
print("Found Goal Cube at: " + str(goalx) + ", " +
str(goaly))
grid.clearGoals()
grid.addGoal((goalx, goaly))
print(grid.getGoals())
repath = True
grid.clearStart()
grid.setStart(botPoint)
grid.clearVisited()
# run astar search
astar(grid, heuristic)
path = grid.getPath()
# where are we now?
if len(path) - path.index(botPoint) == 1:
endPoint = botPoint
else:
endPoint = path[path.index(botPoint) + 1]
print("bot: {0}".format(botPoint))
print("end: {0}".format(endPoint))
#if repath == False:
#get the vector of displacement between the endpoint and the bot's current point
dispVector = (endPoint[0] - botPoint[0], endPoint[1] - botPoint[1])
#get a scalar measure of how far the endpoint is from the bot's current point
dispScalar = math.sqrt(
math.pow(dispVector[0], 2) + math.pow(dispVector[1], 2))
#atan2(y,x) gives the absolute angle of a vector, so we can use it to get the best heading
heading = math.atan2(dispVector[1], dispVector[0])
#we turn based on the difference between the desired heading and the bot's previous heading
turn = heading - previousHeading
previousHeading = heading
if dispScalar != 0:
#turn and move the bot
robot.turn_in_place(radians(turn)).wait_for_completed()
robot.drive_wheels(speed, speed, duration=2 * dispScalar)
if goalCubePos is not None:
goalVector = (goalCubePos[0] - botPoint[0],
goalCubePos[1] - botPoint[1])
goalScalar = math.sqrt(
math.pow(goalVector[0], 2) + math.pow(goalVector[1], 2))
#if we've reached the center of the grid and still haven't found the goal cube, slowly turn in place (until we find it)
if dispScalar == 0 and goalCubePos is None:
robot.turn_in_place(degrees(15)).wait_for_completed()
print("Turning in place til we find the cube")
#if we HAVE found the goal cube, and we've reached the endpoint...
elif dispScalar == 0 and goalCubePos is not None and goalScalar == 0:
#get the heading and turn to face the cube
heading = math.atan2(goalCubePos[1] - endPoint[1],
goalCubePos[0] - endPoint[0])
turn = heading - previousHeading
robot.turn_in_place(radians(turn)).wait_for_completed()
print("Reached goal cube")
#and we're done!
robot.set_all_backpack_lights(cozmo.lights.blue_light)
robot.turn_in_place(degrees(720))
robot.play_audio(cozmo.audio.AudioEvents.SfxGameWin)
time.sleep(5)
break
#update the bot's point to be the endpoint it just navigated to
botPoint = endPoint
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment document for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
robot.image_stream_enabled = True
cube1pos = None
curHeading = 0
# obstacle_size = [-1, 0, 1]
obstacle_size = [-2, -1, 0, 1, 2]
# obstacle_size = [-3, -2, -1, 0, 1, 2, 3]
gridscale = 25
scale = 27
a = (2, 2)
grid.setStart(a)
grid.addGoal((13, 9))
objects = set()
robot.set_head_angle(radians(-0.23)).wait_for_completed()
while not stopevent.is_set():
# print([ e.pose.position for e in robot.world.visible_objects])
# if world.light_cubes[cozmo.objects.LightCube1Id].is_visible
astar(grid, heuristic)
path = grid.getPath()
# print([e for e in grid.getGoals()])
path_ind = path.index(a)
if len(path) - path_ind == 1:
b = a
print("GOOOOOOOOAAAL")
else:
b = path[path_ind + 1]
print("a", a, "b", b)
print(path, path_ind)
for e in robot.world.visible_objects:
if e.object_id not in objects:
xr = a[0]
yr = a[1]
xc = e.pose.position.x
yc = e.pose.position.y
# yc = yr + ((e.pose.position.x * math.sin(curHeading)) + (e.pose.position.y * math.cos(curHeading)))/scale
print(e.pose.position)
xc = math.ceil((xc)/gridscale) + 2
yc = math.ceil((yc)/gridscale) + 2
if xc == 27:
xc = 26
if yc == 27:
yc = 26
print("xr: %d, yr: %d" % (xr, yr))
print("xc: %d, yc: %d" % (xc, yc))
print(curHeading/math.pi)
if xc <= 26 and yc <= 18:
for i in obstacle_size:
for j in obstacle_size:
xci = xc + i
ycj = yc + j
if xci <= 26 and xci >= 0 and ycj <= 18 and ycj >= 0:
grid.addObstacle((xci,ycj))
grid.clearVisited()
grid.setStart(b)
print(grid.getGoals())
print("Obstacle pos", xc,yc)
objects.add(e.object_id) #you ge tthe idea
if robot.world.light_cubes[cozmo.objects.LightCube1Id].object_id == e.object_id:
cube1pos = (xc,yc)
print("e id ",e.object_id)
angelz = e.pose.rotation.angle_z.radians
print("angelz+mod",angelz,angelz+math.pi)
angelz = math.pi + angelz
xc += round(math.cos(angelz)) * (len(obstacle_size)//2 + 1)
yc += round(math.sin(angelz)) * (len(obstacle_size)//2 + 1)
grid.clearGoals()
grid.addGoal((xc, yc))
# else:
# make it an obstacle
# if not cubeFound:
# sleep(.4)
step = (b[0]-a[0], b[1]-a[1])
step_len = math.sqrt(math.pow(step[0],2) + math.pow(step[1],2))
heading = math.atan2(step[1], step[0])
turnHeading = heading - curHeading
curHeading = heading
robot.turn_in_place(radians(turnHeading)).wait_for_completed()
# print(step_len)
robot.drive_wheels(scale, scale, duration=1.4*step_len if step_len == 1 else 1.4*step_len)
# grid.setStart(b)
if step_len == 0 and cube1pos is None:
robot.turn_in_place(radians(math.pi/16)).wait_for_completed()
elif step_len == 0 and cube1pos is not None:
robot.drive_wheels(0, 0, duration=1)
print("cube1pos", cube1pos)
heading = math.atan2(cube1pos[1] - b[1], cube1pos[0] - b[0])
print("heading", heading)
print("curHeading", curHeading)
turnHeading = heading - curHeading
curHeading = heading
robot.turn_in_place(radians(turnHeading)).wait_for_completed()
break
a = b
