Write/Refactor Dock Shape Computer Vision

[kev-the-dev]

Because the Blue/Red/Green Triangle/Circle/Cruciform will likely be in the next RobotX (it was in several challenges in both previous RobotX challenges), we should try to improve this node as much as possible. Here's some ideas I have for the refactor:

• Replace shape matching heuristics with cv2.matchShapes, which worked great on SubjuGator
• Use adaptive Canny parameters for more robust edge detection
• Remove dead code
• integrate LIDAR point matching for 3D information, as I think the current 2 node implementation is a little ugly (or make it both a node AND a library)
• stretch goal: make "shape detection" a generic MIL common tool, where you simply give the program a set of 2D points to match to (using matchShapes)

[kev-the-dev]

Here's what we have right now

[kev-the-dev]

@saltyan007 since you almost have a working ROS integration, let me list what should be done before you PR code so I don't have to repeat myself. These were similar to the problems with your hardware test code, so I'll state the code expectations explicitly here. Remember that learning how to solve problems and research new ideas is the ONLY way to learn to write good code.

• Code must follow PEP8 style guidelines, including NAMING CONVENTIONS. You can easily check this by running navfmt
• Program should be placed with the other python nodes in the vision folder. it should be easy to find where this is
• Program should be extensively commented. If should be clear what each function does and how to use the program. If it is not clear, lines should be commented also saying what they do.
• Program should be written neatly with no "dead"/unused or pointless code. You just be able to justify every line and know what it does!!!
• Program should not open opencv windows by default (this can be enabled with a argument/ROS param)
• any constants like color codes, blur kernel, etc should be ROS params
• code should be committed using git, following our commit message convention you can see in the github history

[kev-the-dev]

An incomplete python script was started in #335