Port fix_build branch for indigo-devel (fix compilation for gazebo4)

I've ported the pull request #221 and target it to indigo. It is not exactly the same, some changes are not needed in the indigo branch.

//cc @nkoenig @hsu @adolfo-rt