Moved code around in different files
Fixed costmap laser scan update