For my lab, I had to implement a clearance based path finding algorithm for the robot I am working with to autonomously navigate and map (using SLAM) a series of hallways with obstacles in the hallway.The next step is to ramp it up to outside navigation, then indoor quad-copter navigation and mapping, then outside quad copter navigation and mapping.
I hope you guys find this useful.
Code: