Now that system is functional, code base should be cleaned, properly refactored to utilize common code, improved to better follow ROS standards, and non-critical bugs should be fixed.