Updated CHANGELOG and package.xml