Roz - Obstacle Scanning

I wrote up a simple scanning routine for Roz that does a 120 degree scan using the front-facing GP2D12 sensor, gathering a result every 40 ms (basically the update rate of the sensor). The scan takes one second, so the resolution ended up being five degrees.

Here's a quick video of the scan itself:

Here's the output of the scanning routine:

>>> import sonarTest
Total time:  1000
[6.369501, 6.369501, 6.354494, 6.622461, 7.411738, 8.793456, 10.49099, 12.54209, 12.52528, 15.2174, 19.15718, 23.08899, 29.92357, 38.45215, 49.59524, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50]

Groups:  [(15, 24)]

Max Group:  (15, 24)
Center of Max Group:  618
Center Angle:  31

The scan routine, after running, analyzes the results, looking for areas where there are no obstacles. Each contiguous area gets grouped together, and it finds the "widest" obstacle free area, and then points the robot's head towards the center of that area at the very end.

Here's a plot of the scan:

I've included scan line numbers in the plot for reference. The wall shows up nicely in the plot.

In the actual obstacle avoidance routine I've written using this, Roz rotates his body to that final direction, and then continues walking.

The obstacle scanner code is here:

https://github.com/JonHylands/roz/blob/master/roz.py#L125-L170

and the place where I use it is:

https://github.com/JonHylands/roz/blob/master/roz.py#L280-L312

I use body inverse kinematics to move Roz's body forwards while scanning. This has the effect of turning the main leg servos back, so the legs aren't in the way of the head as it does its scan.

I'm planning on designing and building a new head eventually, that will replace the front GP2D12 range finder with a LIDAR-lite sensor. I backed their original crowd-funding campaign, so I have one. While that sensor is major overkill in terms of capabilities, the really important thing is it can sample at 250 Hz, which is 10x faster than the GP2D12. This will allow me to get one-degree resolution scans.

I plan on using this capability for more than just finding the way out of tight corners. I eventually want to use it to help choose which direction is best to walk in, and to do a limited amount of localized mapping.