Pull request on the pull request :-)

Few gotchas:
- unit-test not tested :-O
- at least on unrelated commit (for ROS). Feel free to ignore it.