Create an Obstacle Manager

[garethellis0]

This will act like a global map for us, general structure would be:

• take in igvc_obstacle_list message (note that this message type does not yet exist, see #228 )
• compare all the obstacles in the message to all already known obstacles, merge/update any that are similar enough
• purge all obstacles outside of a given radius of the robot (reducing issues with drift due to inaccurate sensor data)
• broadcast a nav_msgs/OccupancyGrid message with all the obstacles on it inflated by some given radius

Note There is a lot of complexity in the above structure that likely needs to be hashed out some more before we implement this. For example:

• how do we know when two obstacles are "similar"?
• how do we merge/update obstacles?

[garethellis0]

After some thought (pictured below):

Determining If Two Obstacles are Close together

• what does it mean to say that two obstacles are "close together"?

  Cones

• fairly easy, just need to check if d = sqrt(d_x^2 + d_y^2) is below some d_max

  Lines

• much harder
• initial plan (bottom right of first image) is to decompose both lines into a series of points and doing some sort of line of best through them, then checking if the sum error from the regression is below some error_max

Merging Two Obstacles

• once we know that two obstacles are the "same", how do we merge them?

  Cones

• pretty easy, we probably just want to take the newer cone, since they are discrete and Lidar is pretty accurate

  Lines

• much harder
• initial plan (bottom of second image) is to decompose the lines into points and do a regression line through them. Priority can be given to the new or old lines (or equally), depending on what error the regression prioritizes minimizing.

My Thoughts (In Text Form)

[raadkhan]

After doing some searching/thinking about lines, (assuming the line segments are a vector of (x,y) points) we could determine if two line segments are the "same" by seeing if two conditions are both true: Condition 1: take dot product of the lines, that gives cosine of angle between them, if the value is above some threshold (close to 1) then we can declare them as "same" in terms of slope. Condition 2: compare how close is new line's start point to the old line's end point or if the line segments intersect, then we can determine if the lines are close enough to be merged.

[garethellis0]

Just to be clear here, we're actually dealing with n-ary polynomial lines, rather then just straight lines.

[garethellis0]

Splines may be a good way of storing a longer line made up of several polynomials: https://en.wikipedia.org/wiki/Spline_(mathematics)

Note to self: Investigate and expand this.

[garethellis0]

After some consideration, here are my thoughts on how to use splines to represent lines (and merge other lines in) a possible additional step (4) might be to recompute the spline such that we only keep points that are in areas of significant change (ie. corners, etc)

[garethellis0]

Resolved last year.