def detectObstacle(robot: cozmo.robot.Robot, cmap, stack, cube):
print("Detect Obstacle State")
# cmap.set_start(Node((robot.pose.position.x, robot.pose.position.y)))
# nodes = [Node((cube.pose.position.x-5, cube.pose.position.y-5)),Node((cube.pose.position.x+5, cube.pose.position.y+5)),Node ((cube.pose.position.x-5, cube.pose.position.y+5)),Node((cube.pose.position.x+5, cube.pose.position.y-5))]
# cmap.add_obstacle(nodes)
robot.findPath() #trigger to go to FindPath state
return robot, stack, cube # returns cozmo
def findTarget(robot: cozmo.robot.Robot, cmap, stack, cube):
# ADD CODE HERE
# Cozmo should move towards the middle and scout the area until the cube is found!
print("Find Target State")
print(robot.pose_angle.degrees)
robot.turn_in_place(degrees(45)).wait_for_completed()
robot.drive_straight(distance_mm(250), speed_mmps(50)).wait_for_completed()
lookAround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
try:
cube = robot.world.wait_for_observed_light_cube()
lookAround.stop()
except asyncio.TimeoutError:
print("did not find cube")
# cube.object_id how you detect which CUBE you are using
if cube is not None:
print(cube)
if cube.object_id == cubeID:
cmap.set_start(Node((robot.pose.position.x, robot.pose.position.y)))
cmap.add_goal(Node((cube.pose.position.x,cube.pose.position.y)))
robot.findPath() # trigger to go to FindPath state
return robot, stack, cube
