Major upgrade. Param names have changes, and some function headers too. Tested with iri_wolf ros node