async def run(robot: cozmo.robot.Robot): global flag_odom_init, last_pose # Obtain odometry information last_pose = robot.last_pose current_odom = compute_odometry(robot.pose) robot.last_pose = robot.pose # Obtain list of currently seen markers and their poses images = await image_processing(robot) r_marker_list = cvt_2Dmarker_measurements(images) # Update the particle filter using the above information result = robot.pf.update(current_odom, r_marker_list) # Determine the robot's action based on current state of localization system # Then do the below. converged = result[3] if robot.played_goal_animation: robot.stop_all_motors() elif converged: # Have the robot drive to the goal. # Goal is defined w/ position & orientation position = result[0:2] rounded_position = tuple( int(position[i]) for i in range(len(position))) angle = math.radians(result[2]) if robot.found_goal: target_angle = 0 else: target_angle = math.degrees( getTurnDirection(math.cos(angle), math.sin(angle), position, goal[0:2])) at_goal = True goal_position = tuple(goal[0:2]) for i in range(len(rounded_position)): if abs(rounded_position[i] - goal_position[i]) > 1: at_goal = False break if at_goal: robot.stop_all_motors() await robot.turn_in_place(degrees(-result[2]), num_retries=3).wait_for_completed() robot.found_goal = True if not robot.played_goal_animation: # Then robot should play a happy animation, and stand still. await robot.say_text("Yay", play_excited_animation=True, duration_scalar=0.5, voice_pitch=1).wait_for_completed() robot.played_goal_animation = True elif abs(target_angle) > robot.TURN_TOLERANCE and abs( 2 * math.pi - abs(target_angle)) > robot.TURN_TOLERANCE: robot.stop_all_motors() await robot.turn_in_place(degrees(target_angle), num_retries=3).wait_for_completed() else: robot.found_goal = False await robot.drive_wheels(robot.ROBOT_SPEED, robot.ROBOT_SPEED, robot.ROBOT_ACCELERATION, robot.ROBOT_ACCELERATION) else: # Have robot actively look around if localization has not converged. await robot.drive_wheels(robot.TURN_SPEED, -robot.TURN_SPEED, robot.ROBOT_ACCELERATION, robot.ROBOT_ACCELERATION) # Make code robust to "kidnapped robot problem" # Reset localization if robot is picked up. # Make robot unhappy. if robot.is_picked_up: robot.pf.__init__(robot.grid) robot.found_goal = False robot.played_goal_animation = False if not robot.played_angry_animation: await play_angry(robot) robot.played_angry_animation = True await robot.set_head_angle(cozmo.util.degrees(robot.HEAD_ANGLE) ).wait_for_completed() else: robot.played_angry_animation = False # Update the particle filter GUI for debugging robot.gui.show_particles(robot.pf.particles) robot.gui.show_mean(*result) robot.gui.updated.set()
async def run(robot: cozmo.robot.Robot): global flag_odom_init, last_pose global grid, gui, pf robot.played_goal_animation = False robot.found_goal = False # start streaming robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() await robot.set_head_angle(cozmo.util.degrees(7)).wait_for_completed() # Obtain the camera intrinsics matrix fx, fy = robot.camera.config.focal_length.x_y cx, cy = robot.camera.config.center.x_y camera_settings = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float) ################### at_goal = False while True: if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text("Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height(1, 10000).wait_for_completed() await robot.set_lift_height(0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue currPose = robot.pose odom = compute_odometry(currPose) markerlist, camera_image = await marker_processing( robot, camera_settings) gui.show_camera_image(camera_image) mean_estimate = pf.update(odom, markerlist) gui.show_camera_image(camera_image) last_pose = currPose estimatex = mean_estimate[0] estimatey = mean_estimate[1] estimateh = mean_estimate[2] converged = mean_estimate[3] # angle = math.radians(mean_estimate[2]) # forward = (math.cos(angle), math.sin(angle)) # target_direction = np.subtract(goal[0:2], mean_estimate[0:2]) # target_angle = math.atan2(target_direction[1], target_direction[0]) - math.atan2(forward[1], forward[0]) distx = goal[0] - mean_estimate[0] disty = goal[1] - mean_estimate[1] angle = math.degrees(math.atan2(disty, distx)) target_angle = diff_heading_deg(angle, mean_estimate[2]) if robot.played_goal_animation: robot.stop_all_motors() if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text("daddy stop helping me. senpai ", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height(1, 10000).wait_for_completed() await robot.set_lift_height(0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue elif converged: #robot.found_goal = True await robot.set_head_angle(degrees(7)).wait_for_completed() print("converged") pos = mean_estimate[0:2] rPos = tuple(int(pos[x]) for x in range(len(pos))) #robot.found_goal =True if robot.found_goal: print("foundgoal") target_angle = 0 if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text( "Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height( 1, 10000).wait_for_completed() await robot.set_lift_height( 0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue #dist = math.sqrt((goal[0] - pos[0])**2 + (goal[1] - pos[1])**2) # robot.stop_all_motors() # robot.drive_straight(distance=dist, speed=10).wait_for_completed() else: print('foundgoal else') # angle = math.radians(mean_estimate[2]) # forward = (math.cos(angle), math.sin(angle)) # target_direction = np.subtract(goal[0:2], mean_estimate[0:2]) # target_angle = math.atan2(target_direction[1], target_direction[0]) - math.atan2(forward[1], forward[0]) distx = goal[0] - mean_estimate[0] disty = goal[1] - mean_estimate[1] angle = math.degrees(math.atan2(disty, distx)) target_angle = diff_heading_deg(angle, mean_estimate[2]) if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text( "Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height( 1, 10000).wait_for_completed() await robot.set_lift_height( 0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue # robot.stop_all_motors() # robot.turn_in_place(degrees(target_angle), num_retries=3).wait_for_completed() # robot.found_goal=True at_goal = True pickupinloop = False gPos = tuple(goal[0:2]) for x in range(len(rPos)): if abs(rPos[x] - gPos[x]) > 1: at_goal = False print("forloop at goal") if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text( "Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height( 1, 10000).wait_for_completed() await robot.set_lift_height( 0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7) ).wait_for_completed() pickupinloop = True break #break if pickupinloop: continue if at_goal: print("at goal") robot.stop_all_motors() await robot.turn_in_place(degrees(-mean_estimate[2]), num_retries=3).wait_for_completed() robot.found_goal = True #if not robot.played_goal_animation: robot.say_text("yeet yeet yeet I DID IT DADDY", play_excited_animation=True, duration_scalar=0.75, voice_pitch=1) robot.played_goal_animation = True #await robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin) elif abs(target_angle) > 2 and abs(2 * math.pi - abs(target_angle)) > 2: print("elif abs(target)angle)") robot.stop_all_motors() await robot.turn_in_place(degrees(target_angle), num_retries=2).wait_for_completed() robot.found_goal = True robot.stop_all_motors() await robot.set_head_angle(degrees(7)).wait_for_completed() if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text( "Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height( 1, 10000).wait_for_completed() await robot.set_lift_height( 0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue else: print("else of at goal") robot.found_goal = False dist = math.sqrt((goal[0] - pos[0])**2 + (goal[1] - pos[1])**2) # if not robot.found_goal: # robot.stop_all_motors() # await robot.turn_in_place(angle=degrees(target_angle),num_retries=2).wait_for_completed() #robot.found_goal = True robot.stop_all_motors() await robot.drive_straight( distance=cozmo.util.distance_mm(dist * 25), speed=cozmo.util.speed_mmps(75)).wait_for_completed() #await robot.drive_wheels(20, 20, duration=6) if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text( "daddy stop helping me. senpai ", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height( 1, 10000).wait_for_completed() await robot.set_lift_height( 0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue else: await robot.set_head_angle(degrees(7)).wait_for_completed() await robot.drive_wheels(-10, 10, duration=0) if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) # robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text( "Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height(1, 10000).wait_for_completed() await robot.set_lift_height(0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue print("turning") if robot.is_picked_up: robot.stop_all_motors() print("picked up") pf = ParticleFilter(grid) #robot.pf = pf robot.found_goal = False robot.played_goal_animation = False robot.at_goal = False if not robot.played_angry_animation: await robot.say_text("Stop holding me daddy uhhh daddy please", duration_scalar=0.75, voice_pitch=1, num_retries=2).wait_for_completed() await robot.set_lift_height(1, 10000).wait_for_completed() await robot.set_lift_height(0, 10000).wait_for_completed() robot.played_angry_animation = True await robot.set_head_angle(degrees(7)).wait_for_completed() continue else: robot.played_angry_animation = False gui.show_particles(pf.particles) gui.show_mean(*mean_estimate) gui.updated.set()