Cambios
- Creando la clase Pose y RobotState
- Cambiando un while(true) por un for en OptimalGridSearch
- Cambiando optimalPathMap por optimalPathVector Nota: posiblemente sea necesario cambiar el parametro de entrada al tipo vector para no tener que hacer la transformacion