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)