Construct "walled" occupancy map from cone positions

[WDobert]

Description

Given a new snapshot of current cone positions, publish an occupancy map that gives a representation of the track with "walls".

The cone positions will be provided by the cone_finder node, publishing ConeList messages on the /cone_finder/cones_found topic.

The node you create should publish nav_msgs/OccupancyGrid messages to the "/map" topic.

Sub-tasks

• [ ] Obtain cone positions from the topic created in #38
• [ ] Given a set of 2D points, create a 2D occupancy grid. Remember that we will be able to make the assumption that the lane width will always be greater than the distance between two cones. So for each cone, you can create lines with all other cones that fall within a certain radius (or a box if easier) of that cone.
• [ ] An algorithm for updating the map during both the first lap and after loop closing.

Resources

• You may want to consider using Bresenham's line algorithm for creating the occupancy grid from the point representation.
• You may have to play around with the resolution of the occupancy grid and the position of the map relative to the kart. For this, I would recommend looking at the map topic published by the map_server node.

[zghera]

Here is my general idea for updating the map at each time step based on the discussion in the meeting today and some more reflection on how to make it a tiny bit more efficient :

Method 1:

Description: More complicated but more efficient as we only re-draw part of the occupancy grid each time.

Data Structures:

• cones: Dict with structure {(x,y): [(x,y), (x,y), ...]}. The key of the dict is a tuple containing two floats representing the (x,y) position of a cone. The value for each key is a list of tuples that are other keys in cones representing the neighbors of this cone. We will draw a wall between this cone and its neighbors.
• cones_to_erase: Dict with the same structure as cones but representing the cones (and their neighbors) to erase from the occupancy grid for the current iteration.
• cones_to_draw: List of (x,y) tuples where each is a key into cones representing the cones that were updated.
• neighbor_thresh: Some number that is slightly larger than the expected distance between cones and much less than the track width.
• update_occupancy_grid: Takes a dict of cones to draw (see structure of cones above) and a flag for whether to erase (set grid square probability to 0) or to fill in (set grid square probability to 100) on the occupancy grid. Squares will be erased/filled in between the coord given by each key and the list of neighbors given by the value for each key in the cones dict provided. Drawing lines on the grid could be accomplished using Bresenham's line algorithm.

Algorithm:

1. Grab a new set of cone positions from the topic in #38. Let's call these points new_cones.
2. (All iterations except the first one) Find the nearest neighbor of every point in new_cones to those in cones. For each point, (call this point new_cone) compare the distance to the nearest neighbor (call this nearest_cone):
   • dist > neighbor_thresh -- add newly seen cones (ensure this does not happen after the first lap):
     • Add the key new_cone into cones and set its value to the list of neighbors that are less than or equal to neighbor_thresh distance away.
     • Add new_cone to cones_to_draw.
   • insignif_thres < dist < neighbor_thresh -- updating cone positions:
     • Copy the list of neighbor cone positions cones[nearest_cone] call this list nearest_cone_neighbors.
     • Pop the old dict entry cone_to_erase = cones.pop('nearest_cone')) and add cone_to_erase to cones_to_erase. C
     • Add the pair {new_cone: nearest_cone_neighbors} to cones.
     • Add new_cone to cones_to_draw.
     • It may also be a good idea to verify the nearest neighbors with the new cone position (although it should probably be okay to skip this).
   • dist < insignif_thres do nothing since cone position difference is negligable.
3. Provide cones_to_erase to update_occupancy_grid with the erase flag set to true.
4. Provide the dict of cones to draw {k: v for k, v in cones.items() if k in cones_to_draw} to update_occupancy_grid with the erase flag set to false.

Method 2:

Description: Simpler but less efficient as we re-draw the entire occupancy grid at each step.

Data Structures:

• cones: List of tuples where each tuple contains two floats representing the (x,y) position of each cone.
• cones_to_update: List of ints where each int is an index into cones representing the cones that were updated.

Algorithm:

1. Grab a new set of cone positions from the topic in #38. Let's call these points new_cones.
2. (All iterations except the first one) Find the nearest neighbor of every point in new_cones to those in cones. For each point, check if the distance to the nearest neighbor is greater than the track width:
   • yes (new cones -- make sure this case does not happen after the first lap): Append this point onto cones.
   • no (updating cones): Add the index of the nearest neighbor in cones to cones_to_update AND update the value in cones with the new position.
3. Feed all cones in cones into the function that creates a 2D occupancy grid from points. This function will simply draw lines between all cones that are within neighbor_thresh distance of each other.

[gfaout]

For the Method 1 implementation-specific approach, when trying to find the nearest cone neighbors within a certain threshold, it might be helpful to use a KD-tree for storing the structures. A rudimentary algorithm that can be modified to fit our needs is here: https://en.wikipedia.org/wiki/K-d_tree#Nearest_neighbour_search. It might be more efficient with a larger number of points, such that we don't have to check every point in cones for every point in new_cones.

[DanMan002]

I also looked into the actual line drawing under the assumption we have the correct two cones to connect, and there is actually a premade python package that does this: https://pypi.org/project/bresenham/. I tested it out on a basic scale and it works fine, so if we can get the first parts of method 1, update_occupancy_grid shouldn't be too much of a problem.

[gfaout]

Looked at the bresenham source code in a little more detail, and one note mentioned in the README was that the following solves the problem faster: skimage.draw.line