Fixed firmware and updated ros_lib