fix refactoring, needs testing with gazebo