async def scan_area(robot: anki_vector.robot.Robot, state: MapState): collect_future = concurrent.futures.Future() # The collect_proximity_data task relies on this external trigger to know when its finished. state.collection_active = True # Activate the collection task while the robot turns in place. collect_task = robot.conn.loop.create_task(collect_proximity_data_loop(robot, collect_future, state)) # Turn around in place, then send the signal to kill the collection task. robot.behavior.turn_in_place(angle=degrees(360.0), speed=degrees(360.0 / PROXIMITY_SCAN_TURN_DURATION_S)) state.collection_active = False # Wait for the collection task to finish. robot.conn.run_coroutine(collect_task) # While the result of the task is not used, this call will propagate any exceptions that # occured in the task, allowing for debug visibility. collect_future.result()
async def map_explorer(robot: anki_vector.robot.Robot): # Drop the lift, so that it does not block the proximity sensor robot.behavior.set_lift_height(0.0) # Create the map state, and add it's rendering function to the viewer's render pipeline state = MapState() robot.viewer_3d.add_render_call(state.render) # Comparison function used for sorting which open nodes are the furthest from all existing # walls and loose contacts. # (Using 1/r^2 to respond strongly to small numbers of close contact and weaking to many distant contacts) def open_point_sort_func(position: Vector3): proximity_sum = 0 for p in state.contact_nodes: proximity_sum = proximity_sum + 1 / (p - position).magnitude_squared for c in state.walls: for p in c.vertices: proximity_sum = proximity_sum + 1 / (p - position).magnitude_squared return proximity_sum # Loop until running out of open samples to navigate to, # or if the process has yet to start (indicated by a lack of cleared_territories). while (state.open_nodes and ACTIVELY_EXPLORE_SPACE) or not state.cleared_territories: if robot.pose: # Delete any open samples range of the robot. state.open_nodes = [position for position in state.open_nodes if (position - robot.pose.position).magnitude > PROXIMITY_SCAN_DISTANCE_THRESHOLD_MM] # Collect map data for the robot's current location. await scan_area(robot, state) # Add where the robot is to the map's cleared territories. state.cleared_territories.append(ClearedTerritory(robot.pose.position, PROXIMITY_SCAN_DISTANCE_THRESHOLD_MM)) # @TODO: This whole block should ideally be replaced with the go_to_pose actions when that is ready to go. # Alternatively, the turn&drive commands can be modified to respond to collisions by cancelling. After # either change, ACTIVELY_EXPLORE_SPACE should be defaulted True if ACTIVELY_EXPLORE_SPACE and state.open_nodes: # Sort the open nodes and find our next navigation point. state.open_nodes.sort(key=open_point_sort_func) nav_point = state.open_nodes[0] # Calculate the distance and direction of this next navigation point. nav_point_delta = Vector3( nav_point.x - robot.pose.position.x, nav_point.y - robot.pose.position.y, 0) nav_distance = nav_point_delta.magnitude nav_direction = nav_point_delta.normalized # Convert the nav_direction into a turn angle relative to the robot's current facing. robot_forward = Vector3(*robot.pose.rotation.to_matrix().forward_xyz).normalized turn_angle = acos(nav_direction.dot(robot_forward)) if nav_direction.cross(robot_forward).z > 0: turn_angle = -turn_angle # Turn toward the nav point, and drive to it. robot.behavior.turn_in_place(angle=radians(turn_angle), speed=degrees(EXPLORE_TURN_SPEED_DPS)) robot.behavior.drive_straight(distance=distance_mm(nav_distance), speed=speed_mmps(EXPLORE_DRIVE_SPEED_MMPS)) if PROXIMITY_EXPLORATION_SHUTDOWN_DELAY_S == 0.0: while True: await asyncio.sleep(1.0) else: print('finished exploring - waiting an additional {0} seconds, then shutting down'.format(PROXIMITY_EXPLORATION_SHUTDOWN_DELAY_S)) await asyncio.sleep(PROXIMITY_EXPLORATION_SHUTDOWN_DELAY_S)