Implemented new modularized code (based on kobuki_driver and kobuki_node)