Fixed some bugs, init working, sometimes needs pre-cleaning the device before running the ROS node