Merge pull request #1 from RoboHAZMAT/master

Sync with master code
Merge pull request #9 from gbledt/master

Sync with master