Openni wrapper - debugging and integration in scitos robot